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

added debug code.

Browse Source
This commit is contained in:
Kayman 2018-10-04 11:05:12 +09:00
parent e1d1314d1e
commit 4da4392881
3 changed files with 25 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -55,6 +55,10 @@ class BallFollower
void waitFollowing();
void startFollowing();
void stopFollowing();
void clearBallPosition()
{
approach_ball_position_ = NotFound;
}
int getBallPosition()
{

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

@ -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_;

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

@ -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<op3_action_module_msgs::IsRunning>("/robotis/action/is_running");
set_joint_module_client_ = nh.serviceClient<robotis_controller_msgs::SetJointModule>("/robotis/set_present_joint_ctrl_modules");
test_pub_ = nh.advertise<std_msgs::String>("/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);
}
}