From 50378f052f1db6eb008d04b5d37c3210535dfefd Mon Sep 17 00:00:00 2001 From: Kayman Date: Thu, 8 Jun 2017 09:19:28 +0900 Subject: [PATCH] cleaned up the code. --- .../include/ball_detector/ball_detector.h | 1 - ball_detector/src/ball_detector_node.cpp | 1 - op3_demo/CMakeLists.txt | 158 +++--------------- op3_demo/include/op3_demo/action_demo.h | 5 +- op3_demo/include/op3_demo/ball_follower.h | 2 +- op3_demo/include/op3_demo/ball_tracker.h | 3 - op3_demo/include/op3_demo/button_test.h | 17 -- op3_demo/include/op3_demo/op_demo.h | 4 - op3_demo/include/op3_demo/soccer_demo.h | 6 +- op3_demo/package.xml | 47 +----- op3_demo/script/action_script.yaml | 26 +-- op3_demo/src/action/action_demo.cpp | 58 +------ op3_demo/src/demo_node.cpp | 42 ++--- op3_demo/src/soccer/ball_follower.cpp | 33 ++-- op3_demo/src/soccer/ball_tracker.cpp | 74 ++------ op3_demo/src/soccer/soccer_demo.cpp | 30 ++-- op3_demo/src/soccer_demo_node.cpp | 20 +-- op3_demo/src/test/button_test.cpp | 91 ---------- op3_demo/src/test/mic_test.cpp | 9 - op3_demo/src/test_node.cpp | 24 --- 20 files changed, 105 insertions(+), 546 deletions(-) diff --git a/ball_detector/include/ball_detector/ball_detector.h b/ball_detector/include/ball_detector/ball_detector.h index da8338e..578d9be 100644 --- a/ball_detector/include/ball_detector/ball_detector.h +++ b/ball_detector/include/ball_detector/ball_detector.h @@ -33,7 +33,6 @@ #ifndef _BALL_DETECTOR_H_ #define _BALL_DETECTOR_H_ -//std #include #include diff --git a/ball_detector/src/ball_detector_node.cpp b/ball_detector/src/ball_detector_node.cpp index a7036cd..d54bb4e 100644 --- a/ball_detector/src/ball_detector_node.cpp +++ b/ball_detector/src/ball_detector_node.cpp @@ -47,7 +47,6 @@ int main(int argc, char **argv) //node loop while (ros::ok()) { - //if new image , do things if (detector.newImage()) { diff --git a/op3_demo/CMakeLists.txt b/op3_demo/CMakeLists.txt index 7d126c5..1a1913c 100644 --- a/op3_demo/CMakeLists.txt +++ b/op3_demo/CMakeLists.txt @@ -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 +################################################################################ \ No newline at end of file diff --git a/op3_demo/include/op3_demo/action_demo.h b/op3_demo/include/op3_demo/action_demo.h index 303acfd..f990ac5 100644 --- a/op3_demo/include/op3_demo/action_demo.h +++ b/op3_demo/include/op3_demo/action_demo.h @@ -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_; }; diff --git a/op3_demo/include/op3_demo/ball_follower.h b/op3_demo/include/op3_demo/ball_follower.h index 632da76..bb5d862 100644 --- a/op3_demo/include/op3_demo/ball_follower.h +++ b/op3_demo/include/op3_demo/ball_follower.h @@ -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_; diff --git a/op3_demo/include/op3_demo/ball_tracker.h b/op3_demo/include/op3_demo/ball_tracker.h index 5fc1181..751309e 100644 --- a/op3_demo/include/op3_demo/ball_tracker.h +++ b/op3_demo/include/op3_demo/ball_tracker.h @@ -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_; diff --git a/op3_demo/include/op3_demo/button_test.h b/op3_demo/include/op3_demo/button_test.h index a858577..2647911 100644 --- a/op3_demo/include/op3_demo/button_test.h +++ b/op3_demo/include/op3_demo/button_test.h @@ -38,11 +38,8 @@ #include #include #include -//#include -//#include #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_; diff --git a/op3_demo/include/op3_demo/op_demo.h b/op3_demo/include/op3_demo/op_demo.h index d68adf5..55e1785 100644 --- a/op3_demo/include/op3_demo/op_demo.h +++ b/op3_demo/include/op3_demo/op_demo.h @@ -43,10 +43,6 @@ class OPDemo { InitPose = 1, WalkingReady = 9, - //GetUpFront = 81, - //GetUpBack = 82, - //RightKick = 83, - //LeftKick = 84, GetUpFront = 122, GetUpBack = 123, RightKick = 121, diff --git a/op3_demo/include/op3_demo/soccer_demo.h b/op3_demo/include/op3_demo/soccer_demo.h index d0d17ce..c7cf790 100644 --- a/op3_demo/include/op3_demo/soccer_demo.h +++ b/op3_demo/include/op3_demo/soccer_demo.h @@ -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 id_joint_table_; std::map joint_id_table_; - bool debug_code_; int wait_count_; bool on_following_ball_; bool restart_soccer_; diff --git a/op3_demo/package.xml b/op3_demo/package.xml index d7f5134..b376b44 100644 --- a/op3_demo/package.xml +++ b/op3_demo/package.xml @@ -6,52 +6,18 @@ op3 default demo It includes three demontrations(soccer demo, vision demo, action script demo) - - - - - kayman - - - - - BSD - - - - - - - - - - - kayman + Pyo - - - - - - - - - - - - - catkin - + catkin roscpp roslib sensor_msgs ball_detector op3_walking_module_msgs cmake_modules - robotis_math - + robotis_math roscpp roslib sensor_msgs @@ -59,11 +25,4 @@ op3_walking_module_msgs cmake_modules robotis_math - - - - - - - \ No newline at end of file diff --git a/op3_demo/script/action_script.yaml b/op3_demo/script/action_script.yaml index 33a7501..077e820 100644 --- a/op3_demo/script/action_script.yaml +++ b/op3_demo/script/action_script.yaml @@ -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] diff --git a/op3_demo/src/action/action_demo.cpp b/op3_demo/src/action/action_demo.cpp index 10c7f88..ecb9255 100644 --- a/op3_demo/src/action/action_demo.cpp +++ b/op3_demo/src/action/action_demo.cpp @@ -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"; diff --git a/op3_demo/src/demo_node.cpp b/op3_demo/src/demo_node.cpp index 6647942..f37a09f 100644 --- a/op3_demo/src/demo_node.cpp +++ b/op3_demo/src/demo_node.cpp @@ -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); } } } diff --git a/op3_demo/src/soccer/ball_follower.cpp b/op3_demo/src/soccer/ball_follower.cpp index 1a4c465..723e9bb 100644 --- a/op3_demo/src/soccer/ball_follower.cpp +++ b/op3_demo/src/soccer/ball_follower.cpp @@ -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; } diff --git a/op3_demo/src/soccer/ball_tracker.cpp b/op3_demo/src/soccer/ball_tracker.cpp index db47c0d..6584bd8 100644 --- a/op3_demo/src/soccer/ball_tracker.cpp +++ b/op3_demo/src/soccer/ball_tracker.cpp @@ -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"); } } diff --git a/op3_demo/src/soccer/soccer_demo.cpp b/op3_demo/src/soccer/soccer_demo.cpp index c3fc7cf..3801cd4 100644 --- a/op3_demo/src/soccer/soccer_demo.cpp +++ b/op3_demo/src/soccer/soccer_demo.cpp @@ -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) diff --git a/op3_demo/src/soccer_demo_node.cpp b/op3_demo/src/soccer_demo_node.cpp index 6332c7b..76d1f39 100644 --- a/op3_demo/src/soccer_demo_node.cpp +++ b/op3_demo/src/soccer_demo_node.cpp @@ -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; diff --git a/op3_demo/src/test/button_test.cpp b/op3_demo/src/test/button_test.cpp index 7421d98..f5c2d61 100644 --- a/op3_demo/src/test/button_test.cpp +++ b/op3_demo/src/test/button_test.cpp @@ -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("/robotis/enable_ctrl_module", 0); -// motion_index_pub_ = nh.advertise("/robotis/action/page_num", 0); rgb_led_pub_ = nh.advertise("/robotis/sync_write_item", 0); play_sound_pub_ = nh.advertise("/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; diff --git a/op3_demo/src/test/mic_test.cpp b/op3_demo/src/test/mic_test.cpp index aeec922..dd0edfd 100644 --- a/op3_demo/src/test/mic_test.cpp +++ b/op3_demo/src/test/mic_test.cpp @@ -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; } diff --git a/op3_demo/src/test_node.cpp b/op3_demo/src/test_node.cpp index 8837c19..b1ab4a2 100644 --- a/op3_demo/src/test_node.cpp +++ b/op3_demo/src/test_node.cpp @@ -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();