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); +} + }