diff --git a/ball_detector/launch/ball_detector_params.yaml b/ball_detector/launch/ball_detector_params.yaml index d6310e1..2992b70 100644 --- a/ball_detector/launch/ball_detector_params.yaml +++ b/ball_detector/launch/ball_detector_params.yaml @@ -7,8 +7,8 @@ hough_accum_th: 28 min_radius: 20 max_radius: 300 filter_h_min: 350 -filter_h_max: 20 -filter_s_min: 220 +filter_h_max: 15 +filter_s_min: 200 filter_s_max: 255 filter_v_min: 80 filter_v_max: 255 diff --git a/op3_demo/include/op3_demo/action_demo.h b/op3_demo/include/op3_demo/action_demo.h index bac1eab..32a9435 100644 --- a/op3_demo/include/op3_demo/action_demo.h +++ b/op3_demo/include/op3_demo/action_demo.h @@ -29,6 +29,7 @@ #include "op3_demo/op_demo.h" #include "robotis_controller_msgs/JointCtrlModule.h" +#include "robotis_controller_msgs/SetModule.h" #include "op3_action_module_msgs/IsRunning.h" namespace robotis_op @@ -86,7 +87,7 @@ class ActionDemo : public OPDemo bool isActionRunning(); void setModuleToDemo(const std::string &module_name); - + void callServiceSettingModule(const std::string &module_name); void actionSetNameCallback(const std_msgs::String::ConstPtr& msg); void buttonHandlerCallback(const std_msgs::String::ConstPtr& msg); void demoCommandCallback(const std_msgs::String::ConstPtr &msg); @@ -99,6 +100,7 @@ class ActionDemo : public OPDemo ros::Subscriber demo_command_sub_; ros::ServiceClient is_running_client_; + ros::ServiceClient set_joint_module_client_; std::map action_sound_table_; std::vector play_list_; diff --git a/op3_demo/include/op3_demo/ball_tracker.h b/op3_demo/include/op3_demo/ball_tracker.h index 2304a43..0d3cfac 100644 --- a/op3_demo/include/op3_demo/ball_tracker.h +++ b/op3_demo/include/op3_demo/ball_tracker.h @@ -26,6 +26,7 @@ #include #include #include +//#include #include #include "robotis_controller_msgs/JointCtrlModule.h" @@ -39,7 +40,7 @@ namespace robotis_op // head tracking for looking the ball class BallTracker { - public: +public: enum TrackingStatus { NotFound = -1, @@ -72,7 +73,7 @@ class BallTracker return current_ball_bottom_; } - protected: +protected: const double FOV_WIDTH; const double FOV_HEIGHT; const int NOT_FOUND_THRESHOLD; @@ -92,6 +93,8 @@ class BallTracker ros::Publisher head_joint_pub_; ros::Publisher head_scan_pub_; + // ros::Publisher error_pub_; + ros::Publisher motion_index_pub_; ros::Subscriber ball_position_sub_; @@ -107,7 +110,9 @@ class BallTracker bool on_tracking_; double current_ball_pan_, current_ball_tilt_; double current_ball_bottom_; + double x_error_sum_, y_error_sum_; ros::Time prev_time_; + double p_gain_, d_gain_, i_gain_; }; } diff --git a/op3_demo/include/op3_demo/soccer_demo.h b/op3_demo/include/op3_demo/soccer_demo.h index d18343f..f65d81b 100644 --- a/op3_demo/include/op3_demo/soccer_demo.h +++ b/op3_demo/include/op3_demo/soccer_demo.h @@ -30,6 +30,8 @@ #include "robotis_math/robotis_linear_algebra.h" #include "op3_action_module_msgs/IsRunning.h" #include "robotis_controller_msgs/SyncWriteItem.h" +#include "robotis_controller_msgs/JointCtrlModule.h" +#include "robotis_controller_msgs/SetJointModule.h" namespace robotis_op { @@ -70,6 +72,7 @@ class SoccerDemo : public OPDemo void setBodyModuleToDemo(const std::string &body_module, bool with_head_control = true); void setModuleToDemo(const std::string &module_name); + void callServiceSettingModule(const robotis_controller_msgs::JointCtrlModule &modules); void parseJointNameFromYaml(const std::string &path); bool getJointNameFromID(const int &id, std::string &joint_name); bool getIDFromJointName(const std::string &joint_name, int &id); @@ -101,6 +104,7 @@ class SoccerDemo : public OPDemo ros::Subscriber imu_data_sub_; ros::ServiceClient is_running_client_; + ros::ServiceClient set_joint_module_client_; std::map id_joint_table_; std::map joint_id_table_; diff --git a/op3_demo/include/op3_demo/vision_demo.h b/op3_demo/include/op3_demo/vision_demo.h index 470962d..3ebdc46 100644 --- a/op3_demo/include/op3_demo/vision_demo.h +++ b/op3_demo/include/op3_demo/vision_demo.h @@ -30,6 +30,7 @@ #include "op3_demo/face_tracker.h" #include "robotis_controller_msgs/SyncWriteItem.h" +#include "robotis_controller_msgs/SetModule.h" namespace robotis_op { @@ -59,6 +60,7 @@ class VisionDemo : public OPDemo 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_; @@ -70,9 +72,9 @@ class VisionDemo : public OPDemo ros::Subscriber buttuon_sub_; ros::Subscriber faceCoord_sub_; + ros::ServiceClient set_joint_module_client_; geometry_msgs::Point face_position_; - bool is_tracking_; int tracking_status_; }; diff --git a/op3_demo/launch/demo.launch b/op3_demo/launch/demo.launch index 391a9f3..aa5098b 100644 --- a/op3_demo/launch/demo.launch +++ b/op3_demo/launch/demo.launch @@ -23,6 +23,8 @@ + + diff --git a/op3_demo/launch/self_test.launch b/op3_demo/launch/self_test.launch index 0f34bca..c5ca857 100644 --- a/op3_demo/launch/self_test.launch +++ b/op3_demo/launch/self_test.launch @@ -17,7 +17,14 @@ + + + - + + + + + diff --git a/op3_demo/src/action/action_demo.cpp b/op3_demo/src/action/action_demo.cpp index 76a238c..4f14679 100644 --- a/op3_demo/src/action/action_demo.cpp +++ b/op3_demo/src/action/action_demo.cpp @@ -53,8 +53,6 @@ void ActionDemo::setDemoEnable() { setModuleToDemo("action_module"); - usleep(100 * 1000); - enable_ = true; ROS_INFO_COND(DEBUG_PRINT, "Start ActionScript Demo"); @@ -69,7 +67,7 @@ void ActionDemo::setDemoDisable() stopProcess(); enable_ = false; - + ROS_WARN("Set Action demo disable"); play_list_.resize(0); } @@ -181,6 +179,7 @@ void ActionDemo::callbackThread() buttuon_sub_ = nh.subscribe("/robotis/open_cr/button", 1, &ActionDemo::buttonHandlerCallback, this); is_running_client_ = nh.serviceClient("/robotis/action/is_running"); + set_joint_module_client_ = nh.serviceClient("/robotis/set_present_ctrl_modules"); while (nh.ok()) { @@ -359,11 +358,22 @@ void ActionDemo::buttonHandlerCallback(const std_msgs::String::ConstPtr& msg) void ActionDemo::setModuleToDemo(const std::string &module_name) { - std_msgs::String control_msg; - control_msg.data = "action_module"; + callServiceSettingModule(module_name); + ROS_INFO_STREAM("enable module : " << module_name); +} - module_control_pub_.publish(control_msg); - std::cout << "enable module : " << module_name << std::endl; +void ActionDemo::callServiceSettingModule(const std::string &module_name) +{ + robotis_controller_msgs::SetModule set_module_srv; + set_module_srv.request.module_name = module_name; + + if (set_joint_module_client_.call(set_module_srv) == false) + { + ROS_ERROR("Failed to set module"); + return; + } + + return ; } void ActionDemo::demoCommandCallback(const std_msgs::String::ConstPtr &msg) diff --git a/op3_demo/src/soccer/ball_follower.cpp b/op3_demo/src/soccer/ball_follower.cpp index f59a23e..eec94e9 100644 --- a/op3_demo/src/soccer/ball_follower.cpp +++ b/op3_demo/src/soccer/ball_follower.cpp @@ -23,7 +23,7 @@ namespace robotis_op BallFollower::BallFollower() : nh_(ros::this_node::getName()), - FOV_WIDTH(26.4 * M_PI / 180), + FOV_WIDTH(35.2 * M_PI / 180), FOV_HEIGHT(21.6 * M_PI / 180), count_not_found_(0), count_to_kick_(0), @@ -216,7 +216,7 @@ bool BallFollower::processFollowing(double x_angle, double y_angle, double ball_ "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("In range [%d]", count_to_kick_); + ROS_INFO("In range [%d | %d]", count_to_kick_, accum_ball_position_); if (count_to_kick_ > 20) { diff --git a/op3_demo/src/soccer/ball_tracker.cpp b/op3_demo/src/soccer/ball_tracker.cpp index b7d9ad2..cbd4e87 100644 --- a/op3_demo/src/soccer/ball_tracker.cpp +++ b/op3_demo/src/soccer/ball_tracker.cpp @@ -22,22 +22,32 @@ namespace robotis_op { BallTracker::BallTracker() - : nh_(ros::this_node::getName()), - FOV_WIDTH(26.4 * 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), - current_ball_bottom_(0), - tracking_status_(NotFound), - DEBUG_PRINT(false) + : 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_pub_ = nh_.advertise("/robotis/head_control/set_joint_states_offset", 0); head_scan_pub_ = nh_.advertise("/robotis/head_control/scan_command", 0); + // error_pub_ = nh_.advertise("/ball_tracker/errors", 0); ball_position_sub_ = nh_.subscribe("/ball_detector_node/circle_set", 1, &BallTracker::ballPositionCallback, this); ball_tracking_command_sub_ = nh_.subscribe("/ball_tracker/command", 1, &BallTracker::ballTrackerCommandCallback, @@ -90,9 +100,10 @@ void BallTracker::stopTracking() on_tracking_ = false; ROS_INFO_COND(DEBUG_PRINT, "Stop Ball tracking"); - double x_error = -atan(ball_position_.x * tan(FOV_WIDTH)); - double y_error = -atan(ball_position_.y * tan(FOV_HEIGHT)); - publishHeadJoint(x_error, y_error); + current_ball_pan_ = 0; + current_ball_tilt_ = 0; + x_error_sum_ = 0; + y_error_sum_ = 0; } void BallTracker::setUsingHeadScan(bool use_scan) @@ -147,24 +158,26 @@ int BallTracker::processTracking() switch (tracking_status) { - case NotFound: - tracking_status_ = tracking_status; - return 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: - x_error = current_ball_pan_ * 0.7; - y_error = current_ball_tilt_ * 0.7; - ball_size = current_ball_bottom_; - break; + 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; + 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; + default: + break; } ROS_INFO_STREAM_COND(DEBUG_PRINT, "--------------------------------------------------------------"); @@ -176,21 +189,34 @@ int BallTracker::processTracking() double delta_time = dur.nsec * 0.000000001 + dur.sec; prev_time_ = curr_time; - // double p_gain = 0.7, d_gain = 0.05; - double p_gain = 0.75, d_gain = 0.04; double x_error_diff = (x_error - current_ball_pan_) / delta_time; double y_error_diff = (y_error - current_ball_tilt_) / delta_time; - 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; + 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_; - ROS_INFO_STREAM_COND(DEBUG_PRINT, "--------------------------------------------------------------"); + // 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); + 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); + 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); @@ -201,7 +227,6 @@ int BallTracker::processTracking() current_ball_bottom_ = ball_size; ball_position_.z = 0; - //count_not_found_ = 0; tracking_status_ = tracking_status; return tracking_status; diff --git a/op3_demo/src/soccer/soccer_demo.cpp b/op3_demo/src/soccer/soccer_demo.cpp index bd887b7..0565152 100644 --- a/op3_demo/src/soccer/soccer_demo.cpp +++ b/op3_demo/src/soccer/soccer_demo.cpp @@ -22,20 +22,20 @@ namespace robotis_op { SoccerDemo::SoccerDemo() - : FALL_FORWARD_LIMIT(60), - FALL_BACK_LIMIT(-60), - SPIN_RATE(30), - DEBUG_PRINT(false), - wait_count_(0), - on_following_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) + : FALL_FORWARD_LIMIT(60), + FALL_BACK_LIMIT(-60), + SPIN_RATE(30), + DEBUG_PRINT(false), + wait_count_(0), + on_following_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; @@ -66,8 +66,6 @@ void SoccerDemo::setDemoEnable() void SoccerDemo::setDemoDisable() { - setModuleToDemo("base_module"); - // handle disable procedure ball_tracker_.stopTracking(); ball_follower_.stopFollowing(); @@ -85,8 +83,10 @@ void SoccerDemo::setDemoDisable() void SoccerDemo::process() { + if(enable_ == false) + return; + // ball tracking - bool is_tracked; int tracking_status; tracking_status = ball_tracker_.processTracking(); @@ -101,9 +101,6 @@ void SoccerDemo::process() wait_count_ = 1 * SPIN_RATE; } - //for debug - //return; - // check to stop if (stop_following_ == true) { @@ -121,20 +118,20 @@ void SoccerDemo::process() { switch(tracking_status) { - case BallTracker::Found: - ball_follower_.processFollowing(ball_tracker_.getPanOfBall(), ball_tracker_.getTiltOfBall(), 0.0); - if(tracking_status_ != tracking_status) - setRGBLED(0x1F, 0x1F, 0x1F); - break; + case BallTracker::Found: + ball_follower_.processFollowing(ball_tracker_.getPanOfBall(), ball_tracker_.getTiltOfBall(), 0.0); + if(tracking_status_ != tracking_status) + setRGBLED(0x1F, 0x1F, 0x1F); + break; - case BallTracker::NotFound: - ball_follower_.waitFollowing(); - if(tracking_status_ != tracking_status) - setRGBLED(0, 0, 0); - break; + case BallTracker::NotFound: + ball_follower_.waitFollowing(); + if(tracking_status_ != tracking_status) + setRGBLED(0, 0, 0); + break; - default: - break; + default: + break; } if(tracking_status != tracking_status_) @@ -144,32 +141,32 @@ void SoccerDemo::process() // check fallen states switch (stand_state_) { - case Stand: + case Stand: + { + // check restart soccer + if (restart_soccer_ == true) { - // check restart soccer - if (restart_soccer_ == true) - { - restart_soccer_ = false; - startSoccerMode(); - break; - } - - // check states for kick - int ball_position = ball_follower_.getBallPosition(); - if (ball_position != robotis_op::BallFollower::NotFound) - { - ball_follower_.stopFollowing(); - handleKick(ball_position); - } + restart_soccer_ = false; + startSoccerMode(); break; } - // fallen state : Fallen_Forward, Fallen_Behind - default: + + // check states for kick + int ball_position = ball_follower_.getBallPosition(); + if (ball_position != robotis_op::BallFollower::NotFound) { ball_follower_.stopFollowing(); - handleFallen(stand_state_); - break; + handleKick(ball_position); } + break; + } + // fallen state : Fallen_Forward, Fallen_Behind + default: + { + ball_follower_.stopFollowing(); + handleFallen(stand_state_); + break; + } } } else @@ -212,6 +209,7 @@ void SoccerDemo::callbackThread() imu_data_sub_ = nh.subscribe("/robotis/open_cr/imu", 1, &SoccerDemo::imuDataCallback, this); is_running_client_ = nh.serviceClient("/robotis/action/is_running"); + set_joint_module_client_ = nh.serviceClient("/robotis/set_present_joint_ctrl_modules"); while (nh.ok()) { @@ -252,12 +250,15 @@ void SoccerDemo::setBodyModuleToDemo(const std::string &body_module, bool with_h if (control_msg.joint_name.size() == 0) return; - module_control_pub_.publish(control_msg); + 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; + robotis_controller_msgs::JointCtrlModule control_msg; std::map::iterator joint_iter; @@ -271,10 +272,25 @@ void SoccerDemo::setModuleToDemo(const std::string &module_name) if (control_msg.joint_name.size() == 0) return; - module_control_pub_.publish(control_msg); + callServiceSettingModule(control_msg); std::cout << "enable module : " << module_name << std::endl; } +void SoccerDemo::callServiceSettingModule(const robotis_controller_msgs::JointCtrlModule &modules) +{ + robotis_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; @@ -414,16 +430,10 @@ void SoccerDemo::startSoccerMode() { setModuleToDemo("action_module"); - usleep(100 * 1000); - playMotion(WalkingReady); - usleep(1500 * 1000); - setBodyModuleToDemo("walking_module"); - usleep(20 * 1000); - ROS_INFO("Start Soccer Demo"); on_following_ball_ = true; start_following_ = true; @@ -445,24 +455,24 @@ void SoccerDemo::handleKick(int ball_position) usleep(1500 * 1000); - if (handleFallen(stand_state_) == true) + if (handleFallen(stand_state_) == true || enable_ == false) return; // kick motion switch (ball_position) { - case robotis_op::BallFollower::OnRight: - std::cout << "Kick Motion [R]: " << ball_position << std::endl; - playMotion(is_grass_ ? RightKick + ForGrass : RightKick); - break; + case robotis_op::BallFollower::OnRight: + std::cout << "Kick Motion [R]: " << ball_position << std::endl; + playMotion(is_grass_ ? RightKick + ForGrass : RightKick); + break; - case robotis_op::BallFollower::OnLeft: - std::cout << "Kick Motion [L]: " << ball_position << std::endl; - playMotion(is_grass_ ? LeftKick + ForGrass : LeftKick); - break; + case robotis_op::BallFollower::OnLeft: + std::cout << "Kick Motion [L]: " << ball_position << std::endl; + playMotion(is_grass_ ? LeftKick + ForGrass : LeftKick); + break; - default: - break; + default: + break; } on_following_ball_ = false; @@ -487,23 +497,21 @@ bool SoccerDemo::handleFallen(int fallen_status) // change to motion module setModuleToDemo("action_module"); - usleep(600 * 1000); - // getup motion switch (fallen_status) { - case Fallen_Forward: - std::cout << "Getup Motion [F]: " << std::endl; - playMotion(is_grass_ ? GetUpFront + ForGrass : GetUpFront); - break; + 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; + case Fallen_Behind: + std::cout << "Getup Motion [B]: " << std::endl; + playMotion(is_grass_ ? GetUpBack + ForGrass : GetUpBack); + break; - default: - break; + default: + break; } while(isActionRunning() == true) diff --git a/op3_demo/src/test_node.cpp b/op3_demo/src/test_node.cpp index 6671097..259ea96 100644 --- a/op3_demo/src/test_node.cpp +++ b/op3_demo/src/test_node.cpp @@ -44,12 +44,17 @@ 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; @@ -75,7 +80,9 @@ int main(int argc, char **argv) init_pose_pub = nh.advertise("/robotis/base/ini_pose", 0); play_sound_pub = nh.advertise("/play_sound_file", 0); led_pub = nh.advertise("/robotis/sync_write_item", 0); + dxl_torque_pub = nh.advertise("/robotis/dxl_torque", 0); ros::Subscriber buttuon_sub = nh.subscribe("/robotis/open_cr/button", 1, buttonHandlerCallback); + ros::Subscriber mode_command_sub = nh.subscribe("/robotis/mode_command", 1, demoModeCommandCallback); default_mp3_path = ros::package::getPath("op3_demo") + "/Data/mp3/"; @@ -93,7 +100,7 @@ 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"); } @@ -120,7 +127,7 @@ int main(int argc, char **argv) goInitPose(); - ROS_INFO("[Go to Demo READY!]"); + ROS_INFO_COND(DEBUG_PRINT, "[Go to Demo READY!]"); break; } @@ -132,7 +139,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; } @@ -143,7 +150,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: @@ -153,7 +160,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; } case ButtonTest: @@ -163,7 +170,7 @@ int main(int argc, char **argv) current_demo = button_test; current_demo->setDemoEnable(); - ROS_INFO("[Start] Button Test"); + ROS_INFO_COND(DEBUG_PRINT, "[Start] Button Test"); break; } case MicTest: @@ -173,7 +180,7 @@ int main(int argc, char **argv) current_demo = mic_test; current_demo->setDemoEnable(); - ROS_INFO("[Start] Mic Test"); + ROS_INFO_COND(DEBUG_PRINT, "[Start] Mic Test"); break; } @@ -205,6 +212,9 @@ int main(int argc, char **argv) void buttonHandlerCallback(const std_msgs::String::ConstPtr& msg) { + if(apply_desired == true) + return; + // in the middle of playing demo if (current_status != Ready) { @@ -236,22 +246,27 @@ void buttonHandlerCallback(const std_msgs::String::ConstPtr& msg) 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; @@ -259,7 +274,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") { @@ -299,7 +314,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); } } } @@ -344,3 +359,63 @@ bool checkManagerRunning(std::string& manager_name) ROS_ERROR("Can't find op3_manager"); return false; } + +void dxlTorqueChecker() +{ + std_msgs::String check_msg; + check_msg.data = "check"; + + dxl_torque_pub.publish(check_msg); +} + +void demoModeCommandCallback(const std_msgs::String::ConstPtr &msg) +{ + // In demo mode + if (current_status != Ready) + { + if (msg->data == "ready") + { + // go to mode selection status + desired_status = Ready; + apply_desired = true; + + playSound(default_mp3_path + "Demonstration ready mode.mp3"); + setLED(0x01 | 0x02 | 0x04); + } + } + // In ready mode + else + { + if(msg->data == "soccer") + { + desired_status = SoccerDemo; + apply_desired = true; + + // play sound + dxlTorqueChecker(); + playSound(default_mp3_path + "Start soccer demonstration.mp3"); + ROS_INFO_COND(DEBUG_PRINT, "= Start Demo Mode : %d", desired_status); + } + else if(msg->data == "vision") + { + desired_status = VisionDemo; + apply_desired = true; + + // play sound + dxlTorqueChecker(); + playSound(default_mp3_path + "Start vision processing demonstration.mp3"); + ROS_INFO_COND(DEBUG_PRINT, "= Start Demo Mode : %d", desired_status); + } + else if(msg->data == "action") + { + desired_status = ActionDemo; + apply_desired = true; + + // play sound + dxlTorqueChecker(); + playSound(default_mp3_path + "Start motion demonstration.mp3"); + ROS_INFO_COND(DEBUG_PRINT, "= Start Demo Mode : %d", desired_status); + } + } +} + diff --git a/op3_demo/src/vision/face_tracker.cpp b/op3_demo/src/vision/face_tracker.cpp index 36a32a4..f9b3e24 100644 --- a/op3_demo/src/vision/face_tracker.cpp +++ b/op3_demo/src/vision/face_tracker.cpp @@ -23,7 +23,7 @@ namespace robotis_op FaceTracker::FaceTracker() : nh_(ros::this_node::getName()), - FOV_WIDTH(26.4 * M_PI / 180), + FOV_WIDTH(35.2 * M_PI / 180), FOV_HEIGHT(21.6 * M_PI / 180), NOT_FOUND_THRESHOLD(50), use_head_scan_(false), diff --git a/op3_demo/src/vision/vision_demo.cpp b/op3_demo/src/vision/vision_demo.cpp index ea91bd8..1062e5c 100644 --- a/op3_demo/src/vision/vision_demo.cpp +++ b/op3_demo/src/vision/vision_demo.cpp @@ -23,7 +23,6 @@ namespace robotis_op VisionDemo::VisionDemo() : SPIN_RATE(30), - is_tracking_(false), tracking_status_(FaceTracker::Waiting) { enable_ = false; @@ -44,18 +43,15 @@ void VisionDemo::setDemoEnable() // change to motion module setModuleToDemo("action_module"); - usleep(100 * 1000); - playMotion(InitPose); usleep(1500 * 1000); setModuleToDemo("head_control_module"); - usleep(20 * 1000); - enable_ = true; + // send command to start face_tracking std_msgs::Bool command; command.data = enable_; face_tracking_command_pub_.publish(command); @@ -70,7 +66,6 @@ void VisionDemo::setDemoDisable() { face_tracker_.stopTracking(); - is_tracking_ = false; tracking_status_ = FaceTracker::Waiting; enable_ = false; @@ -102,9 +97,6 @@ void VisionDemo::process() if(tracking_status != FaceTracker::Waiting) tracking_status_ = tracking_status; - - //is_tracking_ = is_tracked; - //std::cout << "Tracking : " << tracking_status << std::endl; } void VisionDemo::processThread() @@ -136,6 +128,8 @@ void VisionDemo::callbackThread() buttuon_sub_ = nh.subscribe("/robotis/open_cr/button", 1, &VisionDemo::buttonHandlerCallback, this); faceCoord_sub_ = nh.subscribe("/faceCoord", 1, &VisionDemo::facePositionCallback, this); + set_joint_module_client_ = nh.serviceClient("/robotis/set_present_ctrl_modules"); + while (nh.ok()) { ros::spinOnce(); @@ -176,11 +170,22 @@ void VisionDemo::demoCommandCallback(const std_msgs::String::ConstPtr &msg) void VisionDemo::setModuleToDemo(const std::string &module_name) { - std_msgs::String control_msg; - control_msg.data = module_name; + callServiceSettingModule(module_name); + ROS_INFO_STREAM("enable module : " << module_name); +} - module_control_pub_.publish(control_msg); - std::cout << "enable module : " << module_name << std::endl; +void VisionDemo::callServiceSettingModule(const std::string &module_name) +{ + robotis_controller_msgs::SetModule set_module_srv; + set_module_srv.request.module_name = module_name; + + if (set_joint_module_client_.call(set_module_srv) == false) + { + ROS_ERROR("Failed to set module"); + return; + } + + return ; } void VisionDemo::facePositionCallback(const std_msgs::Int32MultiArray::ConstPtr &msg)