BRSRI/bellande_humanoid_robot_intelligence_control_system_architecture
1
0
Fork 0
Code Issues Pull Requests Actions 2 Packages Projects Releases Wiki Activity

added a process thread for ball tracking

Browse Source
This commit is contained in:
Kayman 2018-09-07 10:42:05 +09:00
parent 6c28e17e48
commit 58a62c0721
2 changed files with 47 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
op3_demo/include/op3_demo/soccer_demo.h
View File

@ -72,6 +72,7 @@ class SoccerDemo : public OPDemo
void processThread();
void callbackThread();
void trackingThread();
void setBodyModuleToDemo(const std::string &body_module, bool with_head_control = true);
void setModuleToDemo(const std::string &module_name);
@ -114,7 +115,8 @@ class SoccerDemo : public OPDemo
bool is_grass_;
int wait_count_;
bool on_following_ball_;
bool on_following_ball_;
bool on_tracking_ball_;
bool restart_soccer_;
bool start_following_;
bool stop_following_;

52
op3_demo/src/soccer/soccer_demo.cpp
View File

@ -28,6 +28,7 @@ SoccerDemo::SoccerDemo()
DEBUG_PRINT(false),
wait_count_(0),
on_following_ball_(false),
on_tracking_ball_(false),
restart_soccer_(false),
start_following_(false),
stop_following_(false),
@ -48,6 +49,7 @@ SoccerDemo::SoccerDemo()
boost::thread queue_thread = boost::thread(boost::bind(&SoccerDemo::callbackThread, this));
boost::thread process_thread = boost::thread(boost::bind(&SoccerDemo::processThread, this));
boost::thread tracking_thread = boost::thread(boost::bind(&SoccerDemo::trackingThread, this));
is_grass_ = nh.param<bool>("grass_demo", false);
}
@ -73,6 +75,7 @@ void SoccerDemo::setDemoDisable()
enable_ = false;
wait_count_ = 0;
on_following_ball_ = false;
on_tracking_ball_ = false;
restart_soccer_ = false;
start_following_ = false;
stop_following_ = false;
@ -87,9 +90,9 @@ void SoccerDemo::process()
return;
// ball tracking
int tracking_status;
int tracking_status = tracking_status_;
tracking_status = ball_tracker_.processTracking();
//tracking_status = ball_tracker_.processTracking();
// check to start
if (start_following_ == true)
@ -120,22 +123,22 @@ void SoccerDemo::process()
{
case BallTracker::Found:
ball_follower_.processFollowing(ball_tracker_.getPanOfBall(), ball_tracker_.getTiltOfBall(), 0.0);
if(tracking_status_ != tracking_status)
setRGBLED(0x1F, 0x1F, 0x1F);
// 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);
// if(tracking_status_ != tracking_status)
// setRGBLED(0, 0, 0);
break;
default:
break;
}
if(tracking_status != tracking_status_)
tracking_status_ = tracking_status;
// if(tracking_status != tracking_status_)
// tracking_status_ = tracking_status;
}
// check fallen states
@ -219,6 +222,37 @@ void SoccerDemo::callbackThread()
}
}
void SoccerDemo::trackingThread()
{
if(enable_ == false || on_tracking_ball_ == false)
return;
// ball tracking
int tracking_status;
tracking_status = ball_tracker_.processTracking();
// set led
switch(tracking_status)
{
case BallTracker::Found:
if(tracking_status_ != tracking_status)
setRGBLED(0x1F, 0x1F, 0x1F);
break;
case BallTracker::NotFound:
if(tracking_status_ != tracking_status)
setRGBLED(0, 0, 0);
break;
default:
break;
}
if(tracking_status != tracking_status_)
tracking_status_ = tracking_status;
}
void SoccerDemo::setBodyModuleToDemo(const std::string &body_module, bool with_head_control)
{
robotis_controller_msgs::JointCtrlModule control_msg;
@ -436,6 +470,7 @@ void SoccerDemo::startSoccerMode()
ROS_INFO("Start Soccer Demo");
on_following_ball_ = true;
on_tracking_ball_ = true;
start_following_ = true;
}
@ -443,6 +478,7 @@ void SoccerDemo::stopSoccerMode()
{
ROS_INFO("Stop Soccer Demo");
on_following_ball_ = false;
on_tracking_ball_ = false;
stop_following_ = true;
}