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) {