From 455564ee1da594b3588f9a458f3006b98f674b36 Mon Sep 17 00:00:00 2001 From: Kayman Date: Tue, 2 Oct 2018 10:38:26 +0900 Subject: [PATCH 1/8] changed the soccer demo to track the ball to the end before kicking. --- op3_demo/include/op3_demo/ball_follower.h | 5 +++ op3_demo/include/op3_demo/soccer_demo.h | 1 + op3_demo/src/soccer/soccer_demo.cpp | 49 +++++++++++++++++++++-- 3 files changed, 52 insertions(+), 3 deletions(-) diff --git a/op3_demo/include/op3_demo/ball_follower.h b/op3_demo/include/op3_demo/ball_follower.h index 29857db..f7a93b2 100644 --- a/op3_demo/include/op3_demo/ball_follower.h +++ b/op3_demo/include/op3_demo/ball_follower.h @@ -61,6 +61,11 @@ class BallFollower return approach_ball_position_; } + bool isBallInRange() + { + return (approach_ball_position_ == OnRight || approach_ball_position_ == OnLeft); + } + protected: const bool DEBUG_PRINT; const double CAMERA_HEIGHT; diff --git a/op3_demo/include/op3_demo/soccer_demo.h b/op3_demo/include/op3_demo/soccer_demo.h index f5f6df4..cfbd822 100644 --- a/op3_demo/include/op3_demo/soccer_demo.h +++ b/op3_demo/include/op3_demo/soccer_demo.h @@ -91,6 +91,7 @@ class SoccerDemo : public OPDemo void process(); void handleKick(int ball_position); + void handleKick(); bool handleFallen(int fallen_status); void playMotion(int motion_index); diff --git a/op3_demo/src/soccer/soccer_demo.cpp b/op3_demo/src/soccer/soccer_demo.cpp index 37e2c3f..d8ccf5d 100644 --- a/op3_demo/src/soccer/soccer_demo.cpp +++ b/op3_demo/src/soccer/soccer_demo.cpp @@ -144,10 +144,12 @@ void SoccerDemo::process() // check states for kick int ball_position = ball_follower_.getBallPosition(); - if (ball_position != robotis_op::BallFollower::NotFound) + bool in_range = ball_follower_.isBallInRange(); + + if(in_range == true) { ball_follower_.stopFollowing(); - handleKick(ball_position); + handleKick(); } break; } @@ -490,12 +492,53 @@ void SoccerDemo::handleKick(int ball_position) // change to motion module setModuleToDemo("action_module"); - //usleep(1500 * 1000); + 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::OnLeft: + std::cout << "Kick Motion [L]: " << ball_position << std::endl; + playMotion(is_grass_ ? LeftKick + ForGrass : LeftKick); + break; + + default: + break; + } + + on_following_ball_ = false; + restart_soccer_ = true; + + usleep(2000 * 1000); + + if (handleFallen(stand_state_) == true) + return; + + // ceremony + //playMotion(Ceremony); +} + +void SoccerDemo::handleKick() +{ + usleep(1500 * 1000); + + // change to motion module + setModuleToDemo("action_module"); if (handleFallen(stand_state_) == true || enable_ == false) return; // kick motion + int ball_position = ball_follower_.getBallPosition(); + if(ball_position == BallFollower::NotFound || ball_position == BallFollower::OutOfRange) + return; + switch (ball_position) { case robotis_op::BallFollower::OnRight: From e1d1314d1ea274ef481c05e1215126150bdf61ae Mon Sep 17 00:00:00 2001 From: Kayman Date: Tue, 2 Oct 2018 13:32:51 +0900 Subject: [PATCH 2/8] fixed a bug not to kick --- op3_demo/src/soccer/ball_follower.cpp | 4 ++-- op3_demo/src/soccer/soccer_demo.cpp | 4 ++++ 2 files changed, 6 insertions(+), 2 deletions(-) diff --git a/op3_demo/src/soccer/ball_follower.cpp b/op3_demo/src/soccer/ball_follower.cpp index eec94e9..fa89672 100644 --- a/op3_demo/src/soccer/ball_follower.cpp +++ b/op3_demo/src/soccer/ball_follower.cpp @@ -91,7 +91,7 @@ void BallFollower::startFollowing() void BallFollower::stopFollowing() { on_tracking_ = false; - approach_ball_position_ = NotFound; +// approach_ball_position_ = NotFound; count_to_kick_ = 0; accum_ball_position_ = 0; ROS_INFO("Stop Ball following"); @@ -192,7 +192,7 @@ bool BallFollower::processFollowing(double x_angle, double y_angle, double ball_ ROS_INFO_STREAM_COND(DEBUG_PRINT, "== Head Tilt : " << (current_tilt_ * 180 / M_PI) << " | Ball Y : " << (y_angle * 180 / M_PI)); - approach_ball_position_ = NotFound; + approach_ball_position_ = OutOfRange; double distance_to_ball = CAMERA_HEIGHT * tan(M_PI * 0.5 + current_tilt_ - hip_pitch_offset_ - ball_size); diff --git a/op3_demo/src/soccer/soccer_demo.cpp b/op3_demo/src/soccer/soccer_demo.cpp index d8ccf5d..c3e4626 100644 --- a/op3_demo/src/soccer/soccer_demo.cpp +++ b/op3_demo/src/soccer/soccer_demo.cpp @@ -537,7 +537,11 @@ void SoccerDemo::handleKick() // kick motion int ball_position = ball_follower_.getBallPosition(); if(ball_position == BallFollower::NotFound || ball_position == BallFollower::OutOfRange) + { + on_following_ball_ = false; + restart_soccer_ = true; return; + } switch (ball_position) { From adf821b7e84ea5d2d115a66ca920853ea9b46a21 Mon Sep 17 00:00:00 2001 From: Kayman Date: Tue, 2 Oct 2018 13:33:36 +0900 Subject: [PATCH 3/8] changed parameters for default demo --- op3_ball_detector/cfg/DetectorParams.cfg | 0 op3_ball_detector/cfg/DetectorParamsBlue.cfg | 0 op3_ball_detector/cfg/DetectorParamsRed.cfg | 0 op3_ball_detector/config/ball_detector_params.yaml | 8 ++++---- 4 files changed, 4 insertions(+), 4 deletions(-) mode change 100755 => 100644 op3_ball_detector/cfg/DetectorParams.cfg mode change 100755 => 100644 op3_ball_detector/cfg/DetectorParamsBlue.cfg mode change 100755 => 100644 op3_ball_detector/cfg/DetectorParamsRed.cfg diff --git a/op3_ball_detector/cfg/DetectorParams.cfg b/op3_ball_detector/cfg/DetectorParams.cfg old mode 100755 new mode 100644 diff --git a/op3_ball_detector/cfg/DetectorParamsBlue.cfg b/op3_ball_detector/cfg/DetectorParamsBlue.cfg old mode 100755 new mode 100644 diff --git a/op3_ball_detector/cfg/DetectorParamsRed.cfg b/op3_ball_detector/cfg/DetectorParamsRed.cfg old mode 100755 new mode 100644 diff --git a/op3_ball_detector/config/ball_detector_params.yaml b/op3_ball_detector/config/ball_detector_params.yaml index 49cc203..f7013fb 100644 --- a/op3_ball_detector/config/ball_detector_params.yaml +++ b/op3_ball_detector/config/ball_detector_params.yaml @@ -6,11 +6,11 @@ min_circle_dist: 100 hough_accum_th: 28 min_radius: 20 max_radius: 300 -filter_h_min: 350 -filter_h_max: 15 -filter_s_min: 200 +filter_h_min: 355 +filter_h_max: 25 +filter_s_min: 220 filter_s_max: 255 -filter_v_min: 60 +filter_v_min: 80 filter_v_max: 255 use_second_filter: false filter2_h_min: 30 From 4da43928817cae8d083b81b9b6d02de6cbf66dc8 Mon Sep 17 00:00:00 2001 From: Kayman Date: Thu, 4 Oct 2018 11:05:12 +0900 Subject: [PATCH 4/8] added debug code. --- op3_demo/include/op3_demo/ball_follower.h | 4 ++++ op3_demo/include/op3_demo/soccer_demo.h | 4 ++++ op3_demo/src/soccer/soccer_demo.cpp | 18 +++++++++++++++++- 3 files changed, 25 insertions(+), 1 deletion(-) diff --git a/op3_demo/include/op3_demo/ball_follower.h b/op3_demo/include/op3_demo/ball_follower.h index f7a93b2..fbcc710 100644 --- a/op3_demo/include/op3_demo/ball_follower.h +++ b/op3_demo/include/op3_demo/ball_follower.h @@ -55,6 +55,10 @@ class BallFollower void waitFollowing(); void startFollowing(); void stopFollowing(); + void clearBallPosition() + { + approach_ball_position_ = NotFound; + } int getBallPosition() { diff --git a/op3_demo/include/op3_demo/soccer_demo.h b/op3_demo/include/op3_demo/soccer_demo.h index cfbd822..48fd957 100644 --- a/op3_demo/include/op3_demo/soccer_demo.h +++ b/op3_demo/include/op3_demo/soccer_demo.h @@ -98,6 +98,8 @@ class SoccerDemo : public OPDemo void setRGBLED(int blue, int green, int red); bool isActionRunning(); + void sendDebugTopic(const std::string &msgs); + BallTracker ball_tracker_; BallFollower ball_follower_; @@ -108,6 +110,8 @@ class SoccerDemo : public OPDemo ros::Subscriber demo_command_sub_; ros::Subscriber imu_data_sub_; + ros::Publisher test_pub_; + ros::ServiceClient is_running_client_; ros::ServiceClient set_joint_module_client_; diff --git a/op3_demo/src/soccer/soccer_demo.cpp b/op3_demo/src/soccer/soccer_demo.cpp index c3e4626..42ed395 100644 --- a/op3_demo/src/soccer/soccer_demo.cpp +++ b/op3_demo/src/soccer/soccer_demo.cpp @@ -143,7 +143,7 @@ void SoccerDemo::process() } // check states for kick - int ball_position = ball_follower_.getBallPosition(); +// int ball_position = ball_follower_.getBallPosition(); bool in_range = ball_follower_.isBallInRange(); if(in_range == true) @@ -204,6 +204,8 @@ void SoccerDemo::callbackThread() is_running_client_ = nh.serviceClient("/robotis/action/is_running"); set_joint_module_client_ = nh.serviceClient("/robotis/set_present_joint_ctrl_modules"); + test_pub_ = nh.advertise("/debug_text", 0); + while (nh.ok()) { ros::spinOnce(); @@ -514,6 +516,8 @@ void SoccerDemo::handleKick(int ball_position) on_following_ball_ = false; restart_soccer_ = true; + tracking_status_ = BallTracker::NotFound; + ball_follower_.clearBallPosition(); usleep(2000 * 1000); @@ -540,6 +544,8 @@ void SoccerDemo::handleKick() { on_following_ball_ = false; restart_soccer_ = true; + tracking_status_ = BallTracker::NotFound; + ball_follower_.clearBallPosition(); return; } @@ -561,6 +567,8 @@ void SoccerDemo::handleKick() on_following_ball_ = false; restart_soccer_ = true; + tracking_status_ = BallTracker::NotFound; + ball_follower_.clearBallPosition(); usleep(2000 * 1000); @@ -653,4 +661,12 @@ bool SoccerDemo::isActionRunning() return false; } +void SoccerDemo::sendDebugTopic(const std::string &msgs) +{ + std_msgs::String debug_msg; + debug_msg.data = msgs; + + test_pub_.publish(debug_msg); +} + } From c9f9254e527fb477ef3a483e0758caf66dee058a Mon Sep 17 00:00:00 2001 From: Kayman Date: Mon, 15 Oct 2018 17:54:23 +0900 Subject: [PATCH 5/8] changed a method that judge the ball position. --- op3_demo/include/op3_demo/ball_follower.h | 4 +++- op3_demo/launch/demo.launch | 2 +- op3_demo/src/soccer/ball_follower.cpp | 18 +++++++++++++----- op3_demo/src/soccer/soccer_demo.cpp | 2 ++ 4 files changed, 19 insertions(+), 7 deletions(-) diff --git a/op3_demo/include/op3_demo/ball_follower.h b/op3_demo/include/op3_demo/ball_follower.h index fbcc710..4e2c05f 100644 --- a/op3_demo/include/op3_demo/ball_follower.h +++ b/op3_demo/include/op3_demo/ball_follower.h @@ -20,6 +20,7 @@ #define BALL_FOLLOWER_H_ #include +#include #include #include #include @@ -104,7 +105,7 @@ class BallFollower ros::Publisher head_scan_pub_; ros::Publisher set_walking_command_pub_; ros::Publisher set_walking_param_pub_; - ; + ros::Publisher motion_index_pub_; ros::ServiceClient get_walking_param_client_; @@ -120,6 +121,7 @@ class BallFollower int count_not_found_; int count_to_kick_; int accum_ball_position_; + std::vector ball_position_queue_; bool on_tracking_; int approach_ball_position_; double current_pan_, current_tilt_; diff --git a/op3_demo/launch/demo.launch b/op3_demo/launch/demo.launch index d6d26b4..0bd83a9 100644 --- a/op3_demo/launch/demo.launch +++ b/op3_demo/launch/demo.launch @@ -1,5 +1,5 @@ - + diff --git a/op3_demo/src/soccer/ball_follower.cpp b/op3_demo/src/soccer/ball_follower.cpp index fa89672..0b0826a 100644 --- a/op3_demo/src/soccer/ball_follower.cpp +++ b/op3_demo/src/soccer/ball_follower.cpp @@ -60,7 +60,6 @@ BallFollower::BallFollower() "/robotis/walking/get_params"); prev_time_ = ros::Time::now(); - } BallFollower::~BallFollower() @@ -218,12 +217,21 @@ bool BallFollower::processFollowing(double x_angle, double y_angle, double ball_ ROS_INFO("In range [%d | %d]", count_to_kick_, accum_ball_position_); + // ball queue + if(ball_position_queue_.size() >= 5) + ball_position_queue_.erase(ball_position_queue_.begin()); + + ball_position_queue_.push_back((ball_x_angle > 0) ? 1 : -1); + + if (count_to_kick_ > 20) { setWalkingCommand("stop"); on_tracking_ = false; // check direction of the ball + accum_ball_position_ = std::accumulate(ball_position_queue_.begin(), ball_position_queue_.end(), 0); + if (accum_ball_position_ > 0) { ROS_INFO_COND(DEBUG_PRINT, "Ready to kick : left"); // left @@ -239,10 +247,10 @@ bool BallFollower::processFollowing(double x_angle, double y_angle, double ball_ } else if (count_to_kick_ > 15) { - if (ball_x_angle > 0) - accum_ball_position_ += 1; - else - accum_ball_position_ -= 1; +// if (ball_x_angle > 0) +// accum_ball_position_ += 1; +// else +// accum_ball_position_ -= 1; // send message setWalkingParam(IN_PLACE_FB_STEP, 0, 0); diff --git a/op3_demo/src/soccer/soccer_demo.cpp b/op3_demo/src/soccer/soccer_demo.cpp index 42ed395..638cfdd 100644 --- a/op3_demo/src/soccer/soccer_demo.cpp +++ b/op3_demo/src/soccer/soccer_demo.cpp @@ -553,11 +553,13 @@ void SoccerDemo::handleKick() { case robotis_op::BallFollower::OnRight: std::cout << "Kick Motion [R]: " << ball_position << std::endl; + sendDebugTopic("Kick the ball using Right foot"); playMotion(is_grass_ ? RightKick + ForGrass : RightKick); break; case robotis_op::BallFollower::OnLeft: std::cout << "Kick Motion [L]: " << ball_position << std::endl; + sendDebugTopic("Kick the ball using Left foot"); playMotion(is_grass_ ? LeftKick + ForGrass : LeftKick); break; From a58bb9b30ea1a3012e1d18e8549f81bbe88d6606 Mon Sep 17 00:00:00 2001 From: Kayman Date: Tue, 16 Oct 2018 10:18:49 +0900 Subject: [PATCH 6/8] added a ball decision part before kicking. --- op3_demo/include/op3_demo/ball_follower.h | 1 + op3_demo/src/soccer/ball_follower.cpp | 86 ++++++++++++++--------- op3_demo/src/soccer/soccer_demo.cpp | 1 + 3 files changed, 53 insertions(+), 35 deletions(-) diff --git a/op3_demo/include/op3_demo/ball_follower.h b/op3_demo/include/op3_demo/ball_follower.h index 4e2c05f..90b56fc 100644 --- a/op3_demo/include/op3_demo/ball_follower.h +++ b/op3_demo/include/op3_demo/ball_follower.h @@ -53,6 +53,7 @@ class BallFollower ~BallFollower(); bool processFollowing(double x_angle, double y_angle, double ball_size); + void decideBallPositin(double x_angle, double y_angle); void waitFollowing(); void startFollowing(); void stopFollowing(); diff --git a/op3_demo/src/soccer/ball_follower.cpp b/op3_demo/src/soccer/ball_follower.cpp index 0b0826a..ae412d1 100644 --- a/op3_demo/src/soccer/ball_follower.cpp +++ b/op3_demo/src/soccer/ball_follower.cpp @@ -22,34 +22,34 @@ namespace robotis_op { BallFollower::BallFollower() - : nh_(ros::this_node::getName()), - FOV_WIDTH(35.2 * M_PI / 180), - FOV_HEIGHT(21.6 * M_PI / 180), - count_not_found_(0), - count_to_kick_(0), - on_tracking_(false), - approach_ball_position_(NotFound), - kick_motion_index_(83), - CAMERA_HEIGHT(0.46), - NOT_FOUND_THRESHOLD(50), - MAX_FB_STEP(40.0 * 0.001), - MAX_RL_TURN(15.0 * M_PI / 180), - IN_PLACE_FB_STEP(-3.0 * 0.001), - MIN_FB_STEP(5.0 * 0.001), - MIN_RL_TURN(5.0 * M_PI / 180), - UNIT_FB_STEP(1.0 * 0.001), - UNIT_RL_TURN(0.5 * M_PI / 180), - SPOT_FB_OFFSET(0.0 * 0.001), - SPOT_RL_OFFSET(0.0 * 0.001), - SPOT_ANGLE_OFFSET(0.0), - hip_pitch_offset_(7.0), - current_pan_(-10), - current_tilt_(-10), - current_x_move_(0.005), - current_r_angle_(0), - curr_period_time_(0.6), - accum_period_time_(0.0), - DEBUG_PRINT(false) + : nh_(ros::this_node::getName()), + FOV_WIDTH(35.2 * M_PI / 180), + FOV_HEIGHT(21.6 * M_PI / 180), + count_not_found_(0), + count_to_kick_(0), + on_tracking_(false), + approach_ball_position_(NotFound), + kick_motion_index_(83), + CAMERA_HEIGHT(0.46), + NOT_FOUND_THRESHOLD(50), + MAX_FB_STEP(40.0 * 0.001), + MAX_RL_TURN(15.0 * M_PI / 180), + IN_PLACE_FB_STEP(-3.0 * 0.001), + MIN_FB_STEP(5.0 * 0.001), + MIN_RL_TURN(5.0 * M_PI / 180), + UNIT_FB_STEP(1.0 * 0.001), + UNIT_RL_TURN(0.5 * M_PI / 180), + SPOT_FB_OFFSET(0.0 * 0.001), + SPOT_RL_OFFSET(0.0 * 0.001), + SPOT_ANGLE_OFFSET(0.0), + hip_pitch_offset_(7.0), + current_pan_(-10), + current_tilt_(-10), + current_x_move_(0.005), + current_r_angle_(0), + curr_period_time_(0.6), + accum_period_time_(0.0), + DEBUG_PRINT(false) { current_joint_states_sub_ = nh_.subscribe("/robotis/goal_joint_states", 10, &BallFollower::currentJointStatesCallback, this); @@ -57,7 +57,7 @@ BallFollower::BallFollower() set_walking_command_pub_ = nh_.advertise("/robotis/walking/command", 0); set_walking_param_pub_ = nh_.advertise("/robotis/walking/set_params", 0); get_walking_param_client_ = nh_.serviceClient( - "/robotis/walking/get_params"); + "/robotis/walking/get_params"); prev_time_ = ros::Time::now(); } @@ -90,7 +90,7 @@ void BallFollower::startFollowing() void BallFollower::stopFollowing() { on_tracking_ = false; -// approach_ball_position_ = NotFound; + // approach_ball_position_ = NotFound; count_to_kick_ = 0; accum_ball_position_ = 0; ROS_INFO("Stop Ball following"); @@ -230,7 +230,7 @@ bool BallFollower::processFollowing(double x_angle, double y_angle, double ball_ on_tracking_ = false; // check direction of the ball - accum_ball_position_ = std::accumulate(ball_position_queue_.begin(), ball_position_queue_.end(), 0); + accum_ball_position_ = std::accumulate(ball_position_queue_.begin(), ball_position_queue_.end(), 0); if (accum_ball_position_ > 0) { @@ -247,10 +247,10 @@ bool BallFollower::processFollowing(double x_angle, double y_angle, double ball_ } else if (count_to_kick_ > 15) { -// if (ball_x_angle > 0) -// accum_ball_position_ += 1; -// else -// accum_ball_position_ -= 1; + // if (ball_x_angle > 0) + // accum_ball_position_ += 1; + // else + // accum_ball_position_ -= 1; // send message setWalkingParam(IN_PLACE_FB_STEP, 0, 0); @@ -278,6 +278,22 @@ bool BallFollower::processFollowing(double x_angle, double y_angle, double ball_ return false; } +void BallFollower::decideBallPositin(double x_angle, double y_angle) +{ + // check of getting head joints angle + if (current_tilt_ == -10 && current_pan_ == -10) + { + return; + } + + double ball_x_angle = current_pan_ + x_angle; + + if (ball_x_angle > 0) + approach_ball_position_ = OnLeft; + else + approach_ball_position_ = OnRight; +} + void BallFollower::waitFollowing() { count_not_found_++; diff --git a/op3_demo/src/soccer/soccer_demo.cpp b/op3_demo/src/soccer/soccer_demo.cpp index 638cfdd..6389e15 100644 --- a/op3_demo/src/soccer/soccer_demo.cpp +++ b/op3_demo/src/soccer/soccer_demo.cpp @@ -539,6 +539,7 @@ void SoccerDemo::handleKick() return; // kick motion + ball_follower_.decideBallPositin(ball_tracker_.getPanOfBall(), ball_tracker_.getTiltOfBall()); int ball_position = ball_follower_.getBallPosition(); if(ball_position == BallFollower::NotFound || ball_position == BallFollower::OutOfRange) { From 61650f9e4181161ec398739c275ff339f00af76a Mon Sep 17 00:00:00 2001 From: Kayman Date: Tue, 16 Oct 2018 17:20:39 +0900 Subject: [PATCH 7/8] deleted an unused decision method of ball position. --- op3_demo/include/op3_demo/ball_follower.h | 2 -- op3_demo/src/soccer/ball_follower.cpp | 22 ++++++++++++---------- 2 files changed, 12 insertions(+), 12 deletions(-) diff --git a/op3_demo/include/op3_demo/ball_follower.h b/op3_demo/include/op3_demo/ball_follower.h index 90b56fc..232c6e7 100644 --- a/op3_demo/include/op3_demo/ball_follower.h +++ b/op3_demo/include/op3_demo/ball_follower.h @@ -121,8 +121,6 @@ class BallFollower int count_not_found_; int count_to_kick_; - int accum_ball_position_; - std::vector ball_position_queue_; bool on_tracking_; int approach_ball_position_; double current_pan_, current_tilt_; diff --git a/op3_demo/src/soccer/ball_follower.cpp b/op3_demo/src/soccer/ball_follower.cpp index ae412d1..fdd0d20 100644 --- a/op3_demo/src/soccer/ball_follower.cpp +++ b/op3_demo/src/soccer/ball_follower.cpp @@ -92,7 +92,7 @@ void BallFollower::stopFollowing() on_tracking_ = false; // approach_ball_position_ = NotFound; count_to_kick_ = 0; - accum_ball_position_ = 0; +// accum_ball_position_ = 0; ROS_INFO("Stop Ball following"); setWalkingCommand("stop"); @@ -172,7 +172,7 @@ bool BallFollower::processFollowing(double x_angle, double y_angle, double ball_ prev_time_ = curr_time; count_not_found_ = 0; - int ball_position_sum = 0; +// int ball_position_sum = 0; // check of getting head joints angle if (current_tilt_ == -10 && current_pan_ == -10) @@ -213,15 +213,15 @@ bool BallFollower::processFollowing(double x_angle, double y_angle, double ball_ "head pan : " << (current_pan_ * 180 / M_PI) << " | ball pan : " << (x_angle * 180 / M_PI)); ROS_INFO_STREAM_COND(DEBUG_PRINT, "head tilt : " << (current_tilt_ * 180 / M_PI) << " | ball tilt : " << (y_angle * 180 / M_PI)); - ROS_INFO_STREAM_COND(DEBUG_PRINT, "foot to kick : " << accum_ball_position_); + ROS_INFO_STREAM_COND(DEBUG_PRINT, "foot to kick : " << ball_x_angle); - ROS_INFO("In range [%d | %d]", count_to_kick_, accum_ball_position_); + ROS_INFO("In range [%d | %d]", count_to_kick_, ball_x_angle); // ball queue - if(ball_position_queue_.size() >= 5) - ball_position_queue_.erase(ball_position_queue_.begin()); +// if(ball_position_queue_.size() >= 5) +// ball_position_queue_.erase(ball_position_queue_.begin()); - ball_position_queue_.push_back((ball_x_angle > 0) ? 1 : -1); +// ball_position_queue_.push_back((ball_x_angle > 0) ? 1 : -1); if (count_to_kick_ > 20) @@ -230,9 +230,10 @@ bool BallFollower::processFollowing(double x_angle, double y_angle, double ball_ on_tracking_ = false; // check direction of the ball - accum_ball_position_ = std::accumulate(ball_position_queue_.begin(), ball_position_queue_.end(), 0); +// accum_ball_position_ = std::accumulate(ball_position_queue_.begin(), ball_position_queue_.end(), 0); - if (accum_ball_position_ > 0) +// if (accum_ball_position_ > 0) + if (ball_x_angle > 0) { ROS_INFO_COND(DEBUG_PRINT, "Ready to kick : left"); // left approach_ball_position_ = OnLeft; @@ -261,7 +262,7 @@ bool BallFollower::processFollowing(double x_angle, double y_angle, double ball_ else { count_to_kick_ = 0; - accum_ball_position_ = 0; +// accum_ball_position_ = NotFound; } double fb_move = 0.0, rl_angle = 0.0; @@ -283,6 +284,7 @@ void BallFollower::decideBallPositin(double x_angle, double y_angle) // check of getting head joints angle if (current_tilt_ == -10 && current_pan_ == -10) { + approach_ball_position_ = NotFound; return; } From e2bb17c306ce9feb0d651ecfbab5d1c4c0e5be56 Mon Sep 17 00:00:00 2001 From: Kayman Date: Wed, 17 Oct 2018 14:11:59 +0900 Subject: [PATCH 8/8] changed file permission --- op3_ball_detector/cfg/DetectorParams.cfg | 0 op3_ball_detector/cfg/DetectorParamsBlue.cfg | 0 op3_ball_detector/cfg/DetectorParamsRed.cfg | 0 3 files changed, 0 insertions(+), 0 deletions(-) mode change 100644 => 100755 op3_ball_detector/cfg/DetectorParams.cfg mode change 100644 => 100755 op3_ball_detector/cfg/DetectorParamsBlue.cfg mode change 100644 => 100755 op3_ball_detector/cfg/DetectorParamsRed.cfg diff --git a/op3_ball_detector/cfg/DetectorParams.cfg b/op3_ball_detector/cfg/DetectorParams.cfg old mode 100644 new mode 100755 diff --git a/op3_ball_detector/cfg/DetectorParamsBlue.cfg b/op3_ball_detector/cfg/DetectorParamsBlue.cfg old mode 100644 new mode 100755 diff --git a/op3_ball_detector/cfg/DetectorParamsRed.cfg b/op3_ball_detector/cfg/DetectorParamsRed.cfg old mode 100644 new mode 100755