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

Merge pull request #26 from ROBOTIS-GIT/develop

Browse Source 
Develop
This commit is contained in:
Kayman
2018-10-17 14:38:45 +09:00
committed by GitHub
parent e869a2bf6f e2bb17c306
commit 77d76182b3
6 changed files with 161 additions and 54 deletions
Download Patch File Download Diff File Expand all files Collapse all files

8
op3_ball_detector/config/ball_detector_params.yaml
View File

@@ -6,11 +6,11 @@ min_circle_dist: 100
hough_accum_th: 28
min_radius: 20
max_radius: 300
filter_h_min: 350
filter_h_max: 15
filter_s_min: 200
filter_h_min: 355
filter_h_max: 25
filter_s_min: 220
filter_s_max: 255
filter_v_min: 60
filter_v_min: 80
filter_v_max: 255
use_second_filter: false
filter2_h_min: 30

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

@@ -20,6 +20,7 @@
#define BALL_FOLLOWER_H_
#include <math.h>
#include <numeric>
#include <ros/ros.h>
#include <ros/package.h>
#include <std_msgs/String.h>
@@ -52,15 +53,25 @@ 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();
void clearBallPosition()
{
approach_ball_position_ = NotFound;
}
int getBallPosition()
{
return approach_ball_position_;
}
bool isBallInRange()
{
return (approach_ball_position_ == OnRight || approach_ball_position_ == OnLeft);
}
protected:
const bool DEBUG_PRINT;
const double CAMERA_HEIGHT;
@@ -95,7 +106,7 @@ class BallFollower
ros::Publisher head_scan_pub_;
ros::Publisher set_walking_command_pub_;
ros::Publisher set_walking_param_pub_;
;
ros::Publisher motion_index_pub_;
ros::ServiceClient get_walking_param_client_;
@@ -110,7 +121,6 @@ class BallFollower
int count_not_found_;
int count_to_kick_;
int accum_ball_position_;
bool on_tracking_;
int approach_ball_position_;
double current_pan_, current_tilt_;

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

@@ -91,12 +91,15 @@ class SoccerDemo : public OPDemo
void process();
void handleKick(int ball_position);
void handleKick();
bool handleFallen(int fallen_status);
void playMotion(int motion_index);
void setRGBLED(int blue, int green, int red);
bool isActionRunning();
void sendDebugTopic(const std::string &msgs);
BallTracker ball_tracker_;
BallFollower ball_follower_;
@@ -107,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_;

2
op3_demo/launch/demo.launch
View File

@@ -1,5 +1,5 @@
<?xml version="1.0"?>
<launch>
<launch>
<!-- robotis op3 manager -->
<include file="$(find op3_manager)/launch/op3_manager.launch"/>

110
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,10 +57,9 @@ 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();
}
BallFollower::~BallFollower()
@@ -91,9 +90,9 @@ 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;
// accum_ball_position_ = 0;
ROS_INFO("Stop Ball following");
setWalkingCommand("stop");
@@ -173,7 +172,7 @@ bool BallFollower::processFollowing(double x_angle, double y_angle, double ball_
prev_time_ = curr_time;
count_not_found_ = 0;
int ball_position_sum = 0;
// int ball_position_sum = 0;
// check of getting head joints angle
if (current_tilt_ == -10 && current_pan_ == -10)
@@ -192,7 +191,7 @@ bool BallFollower::processFollowing(double x_angle, double y_angle, double ball_
ROS_INFO_STREAM_COND(DEBUG_PRINT,
"== Head Tilt : " << (current_tilt_ * 180 / M_PI) << " | Ball Y : " << (y_angle * 180 / M_PI));
approach_ball_position_ = NotFound;
approach_ball_position_ = OutOfRange;
double distance_to_ball = CAMERA_HEIGHT * tan(M_PI * 0.5 + current_tilt_ - hip_pitch_offset_ - ball_size);
@@ -214,9 +213,16 @@ bool BallFollower::processFollowing(double x_angle, double y_angle, double ball_
"head pan : " << (current_pan_ * 180 / M_PI) << " | ball pan : " << (x_angle * 180 / M_PI));
ROS_INFO_STREAM_COND(DEBUG_PRINT,
"head tilt : " << (current_tilt_ * 180 / M_PI) << " | ball tilt : " << (y_angle * 180 / M_PI));
ROS_INFO_STREAM_COND(DEBUG_PRINT, "foot to kick : " << accum_ball_position_);
ROS_INFO_STREAM_COND(DEBUG_PRINT, "foot to kick : " << ball_x_angle);
ROS_INFO("In range [%d | %d]", count_to_kick_, ball_x_angle);
// ball queue
// if(ball_position_queue_.size() >= 5)
// ball_position_queue_.erase(ball_position_queue_.begin());
// ball_position_queue_.push_back((ball_x_angle > 0) ? 1 : -1);
ROS_INFO("In range [%d | %d]", count_to_kick_, accum_ball_position_);
if (count_to_kick_ > 20)
{
@@ -224,7 +230,10 @@ bool BallFollower::processFollowing(double x_angle, double y_angle, double ball_
on_tracking_ = false;
// check direction of the ball
if (accum_ball_position_ > 0)
// accum_ball_position_ = std::accumulate(ball_position_queue_.begin(), ball_position_queue_.end(), 0);
// if (accum_ball_position_ > 0)
if (ball_x_angle > 0)
{
ROS_INFO_COND(DEBUG_PRINT, "Ready to kick : left"); // left
approach_ball_position_ = OnLeft;
@@ -239,10 +248,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);
@@ -253,7 +262,7 @@ bool BallFollower::processFollowing(double x_angle, double y_angle, double ball_
else
{
count_to_kick_ = 0;
accum_ball_position_ = 0;
// accum_ball_position_ = NotFound;
}
double fb_move = 0.0, rl_angle = 0.0;
@@ -270,6 +279,23 @@ 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)
{
approach_ball_position_ = NotFound;
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_++;

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

@@ -143,11 +143,13 @@ void SoccerDemo::process()
}
// check states for kick
int ball_position = ball_follower_.getBallPosition();
if (ball_position != robotis_op::BallFollower::NotFound)
// int ball_position = ball_follower_.getBallPosition();
bool in_range = ball_follower_.isBallInRange();
if(in_range == true)
{
ball_follower_.stopFollowing();
handleKick(ball_position);
handleKick();
}
break;
}
@@ -202,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();
@@ -490,8 +494,6 @@ void SoccerDemo::handleKick(int ball_position)
// change to motion module
setModuleToDemo("action_module");
//usleep(1500 * 1000);
if (handleFallen(stand_state_) == true || enable_ == false)
return;
@@ -514,6 +516,62 @@ void SoccerDemo::handleKick(int ball_position)
on_following_ball_ = false;
restart_soccer_ = true;
tracking_status_ = BallTracker::NotFound;
ball_follower_.clearBallPosition();
usleep(2000 * 1000);
if (handleFallen(stand_state_) == true)
return;
// ceremony
//playMotion(Ceremony);
}
void SoccerDemo::handleKick()
{
usleep(1500 * 1000);
// change to motion module
setModuleToDemo("action_module");
if (handleFallen(stand_state_) == true || enable_ == false)
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)
{
on_following_ball_ = false;
restart_soccer_ = true;
tracking_status_ = BallTracker::NotFound;
ball_follower_.clearBallPosition();
return;
}
switch (ball_position)
{
case robotis_op::BallFollower::OnRight:
std::cout << "Kick Motion [R]: " << ball_position << std::endl;
sendDebugTopic("Kick the ball using Right foot");
playMotion(is_grass_ ? RightKick + ForGrass : RightKick);
break;
case robotis_op::BallFollower::OnLeft:
std::cout << "Kick Motion [L]: " << ball_position << std::endl;
sendDebugTopic("Kick the ball using Left foot");
playMotion(is_grass_ ? LeftKick + ForGrass : LeftKick);
break;
default:
break;
}
on_following_ball_ = false;
restart_soccer_ = true;
tracking_status_ = BallTracker::NotFound;
ball_follower_.clearBallPosition();
usleep(2000 * 1000);
@@ -606,4 +664,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);
}
}