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

Merge pull request #24 from ROBOTIS-GIT/develop

Browse Source 
Develop
This commit is contained in:
Kayman 2018-10-01 14:42:46 +09:00 committed by GitHub
commit e869a2bf6f
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
9 changed files with 136 additions and 43 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -43,8 +43,9 @@ class BallFollower
enum
{
NotFound = 0,
OnRight = 1,
OnLeft = 2,
OutOfRange = 1,
OnRight = 2,
OnLeft = 3,
};
BallFollower();

2
op3_demo/include/op3_demo/ball_tracker.h
View File

@ -56,6 +56,7 @@ public:
void stopTracking();
void setUsingHeadScan(bool use_scan);
void goInit();
double getPanOfBall()
{
@ -89,6 +90,7 @@ protected:
//image publisher/subscriber
ros::Publisher module_control_pub_;
ros::Publisher head_joint_offset_pub_;
ros::Publisher head_joint_pub_;
ros::Publisher head_scan_pub_;

2
op3_demo/include/op3_demo/face_tracker.h
View File

@ -54,6 +54,7 @@ class FaceTracker
void setUsingHeadScan(bool use_scan);
void setFacePosition(geometry_msgs::Point &face_position);
void goInit(double init_pan, double init_tile);
double getPanOfFace()
{
@ -79,6 +80,7 @@ class FaceTracker
//image publisher/subscriber
ros::Publisher module_control_pub_;
ros::Publisher head_joint_offset_pub_;
ros::Publisher head_joint_pub_;
ros::Publisher head_scan_pub_;

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

3
op3_demo/include/op3_demo/vision_demo.h
View File

@ -45,6 +45,7 @@ class VisionDemo : public OPDemo
protected:
const int SPIN_RATE;
const int TIME_TO_INIT;
void processThread();
void callbackThread();
@ -74,6 +75,8 @@ class VisionDemo : public OPDemo
ros::ServiceClient set_joint_module_client_;
geometry_msgs::Point face_position_;
ros::Time prev_time_;
int tracking_status_;
};

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

@ -45,7 +45,8 @@ BallTracker::BallTracker()
ROS_INFO_STREAM("Ball tracking Gain : " << p_gain_ << ", " << i_gain_ << ", " << d_gain_);
head_joint_pub_ = nh_.advertise<sensor_msgs::JointState>("/robotis/head_control/set_joint_states_offset", 0);
head_joint_offset_pub_ = nh_.advertise<sensor_msgs::JointState>("/robotis/head_control/set_joint_states_offset", 0);
head_joint_pub_ = nh_.advertise<sensor_msgs::JointState>("/robotis/head_control/set_joint_states", 0);
head_scan_pub_ = nh_.advertise<std_msgs::String>("/robotis/head_control/scan_command", 0);
// error_pub_ = nh_.advertise<std_msgs::Float64MultiArray>("/ball_tracker/errors", 0);
@ -97,6 +98,8 @@ void BallTracker::startTracking()
void BallTracker::stopTracking()
{
goInit();
on_tracking_ = false;
ROS_INFO_COND(DEBUG_PRINT, "Stop Ball tracking");
@ -246,6 +249,19 @@ void BallTracker::publishHeadJoint(double pan, double tilt)
head_angle_msg.position.push_back(pan);
head_angle_msg.position.push_back(tilt);
head_joint_offset_pub_.publish(head_angle_msg);
}
void BallTracker::goInit()
{
sensor_msgs::JointState head_angle_msg;
head_angle_msg.name.push_back("head_pan");
head_angle_msg.name.push_back("head_tilt");
head_angle_msg.position.push_back(0.0);
head_angle_msg.position.push_back(0.0);
head_joint_pub_.publish(head_angle_msg);
}

67
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;
@ -86,11 +89,6 @@ void SoccerDemo::process()
if(enable_ == false)
return;
// ball tracking
int tracking_status;
tracking_status = ball_tracker_.processTracking();
// check to start
if (start_following_ == true)
{
@ -116,26 +114,19 @@ void SoccerDemo::process()
// ball following
if (on_following_ball_ == true)
{
switch(tracking_status)
switch(tracking_status_)
{
case BallTracker::Found:
ball_follower_.processFollowing(ball_tracker_.getPanOfBall(), ball_tracker_.getTiltOfBall(), 0.0);
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);
break;
default:
break;
}
if(tracking_status != tracking_status_)
tracking_status_ = tracking_status;
}
// check fallen states
@ -219,6 +210,50 @@ void SoccerDemo::callbackThread()
}
}
void SoccerDemo::trackingThread()
{
//set node loop rate
ros::Rate loop_rate(SPIN_RATE);
ball_tracker_.startTracking();
//node loop
while (ros::ok())
{
if(enable_ == true && on_tracking_ball_ == true)
{
// 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;
}
//relax to fit output rate
loop_rate.sleep();
}
}
void SoccerDemo::setBodyModuleToDemo(const std::string &body_module, bool with_head_control)
{
robotis_controller_msgs::JointCtrlModule control_msg;
@ -436,6 +471,7 @@ void SoccerDemo::startSoccerMode()
ROS_INFO("Start Soccer Demo");
on_following_ball_ = true;
on_tracking_ball_ = true;
start_following_ = true;
}
@ -443,17 +479,18 @@ void SoccerDemo::stopSoccerMode()
{
ROS_INFO("Stop Soccer Demo");
on_following_ball_ = false;
on_tracking_ball_ = false;
stop_following_ = true;
}
void SoccerDemo::handleKick(int ball_position)
{
usleep(1000 * 1000);
usleep(1500 * 1000);
// change to motion module
setModuleToDemo("action_module");
usleep(1500 * 1000);
//usleep(1500 * 1000);
if (handleFallen(stand_state_) == true || enable_ == false)
return;

18
op3_demo/src/vision/face_tracker.cpp
View File

@ -30,7 +30,8 @@ FaceTracker::FaceTracker()
count_not_found_(0),
on_tracking_(false)
{
head_joint_pub_ = nh_.advertise<sensor_msgs::JointState>("/robotis/head_control/set_joint_states_offset", 0);
head_joint_offset_pub_ = nh_.advertise<sensor_msgs::JointState>("/robotis/head_control/set_joint_states_offset", 0);
head_joint_pub_ = nh_.advertise<sensor_msgs::JointState>("/robotis/head_control/set_joint_states", 0);
head_scan_pub_ = nh_.advertise<std_msgs::String>("/robotis/head_control/scan_command", 0);
face_position_sub_ = nh_.subscribe("/face_position", 1, &FaceTracker::facePositionCallback, this);
@ -96,6 +97,19 @@ void FaceTracker::setFacePosition(geometry_msgs::Point &face_position)
}
}
void FaceTracker::goInit(double init_pan, double init_tile)
{
sensor_msgs::JointState head_angle_msg;
head_angle_msg.name.push_back("head_pan");
head_angle_msg.name.push_back("head_tilt");
head_angle_msg.position.push_back(init_pan);
head_angle_msg.position.push_back(init_tile);
head_joint_pub_.publish(head_angle_msg);
}
int FaceTracker::processTracking()
{
if (on_tracking_ == false)
@ -172,7 +186,7 @@ void FaceTracker::publishHeadJoint(double pan, double tilt)
head_angle_msg.position.push_back(pan);
head_angle_msg.position.push_back(tilt);
head_joint_pub_.publish(head_angle_msg);
head_joint_offset_pub_.publish(head_angle_msg);
}
void FaceTracker::scanFace()

60
op3_demo/src/vision/vision_demo.cpp
View File

@ -22,8 +22,9 @@ namespace robotis_op
{
VisionDemo::VisionDemo()
: SPIN_RATE(30),
tracking_status_(FaceTracker::Waiting)
: SPIN_RATE(30),
TIME_TO_INIT(10),
tracking_status_(FaceTracker::Waiting)
{
enable_ = false;
@ -40,6 +41,9 @@ VisionDemo::~VisionDemo()
void VisionDemo::setDemoEnable()
{
// set prev time for timer
prev_time_ = ros::Time::now();
// change to motion module
setModuleToDemo("action_module");
@ -78,22 +82,34 @@ void VisionDemo::process()
{
int tracking_status = face_tracker_.processTracking();
if(tracking_status_ != tracking_status)
switch(tracking_status)
{
switch(tracking_status)
case FaceTracker::Found:
if(tracking_status_ != tracking_status)
setRGBLED(0x1F, 0x1F, 0x1F);
prev_time_ = ros::Time::now();
break;
case FaceTracker::NotFound:
{
if(tracking_status_ != tracking_status)
setRGBLED(0, 0, 0);
ros::Time curr_time = ros::Time::now();
ros::Duration dur = curr_time - prev_time_;
if(dur.sec > TIME_TO_INIT)
{
case FaceTracker::Found:
setRGBLED(0x1F, 0x1F, 0x1F);
break;
case FaceTracker::NotFound:
setRGBLED(0, 0, 0);
break;
default:
break;
face_tracker_.goInit(0,0);
prev_time_ = curr_time;
}
break;
}
default:
break;
}
if(tracking_status != FaceTracker::Waiting)
tracking_status_ = tracking_status;
@ -176,16 +192,16 @@ void VisionDemo::setModuleToDemo(const std::string &module_name)
void VisionDemo::callServiceSettingModule(const std::string &module_name)
{
robotis_controller_msgs::SetModule set_module_srv;
set_module_srv.request.module_name = module_name;
robotis_controller_msgs::SetModule set_module_srv;
set_module_srv.request.module_name = module_name;
if (set_joint_module_client_.call(set_module_srv) == false)
{
ROS_ERROR("Failed to set module");
return;
}
if (set_joint_module_client_.call(set_module_srv) == false)
{
ROS_ERROR("Failed to set module");
return;
}
return ;
return ;
}
void VisionDemo::facePositionCallback(const std_msgs::Int32MultiArray::ConstPtr &msg)