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

Merge branch 'develop' of https://github.com/ROBOTIS-GIT/ROBOTIS-OP3-Demo into develop

Browse Source
This commit is contained in:
Kayman 2018-10-16 10:20:34 +09:00
commit a38aab15ca
3 changed files with 53 additions and 35 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -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();

86
op3_demo/src/soccer/ball_follower.cpp
View File

@ -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<std_msgs::String>("/robotis/walking/command", 0);
set_walking_param_pub_ = nh_.advertise<op3_walking_module_msgs::WalkingParam>("/robotis/walking/set_params", 0);
get_walking_param_client_ = nh_.serviceClient<op3_walking_module_msgs::GetWalkingParam>(
"/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_++;

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

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