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

cleanup the code.

Browse Source 
improved robotis op3 default demo.
This commit is contained in:
Kayman
2017-07-18 11:37:02 +09:00
parent 464ef98717
commit 54433c57cd
9 changed files with 215 additions and 159 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -54,10 +54,17 @@ namespace robotis_op
class BallTracker
{
public:
enum TrackingStatus
{
NotFound = -1,
Waiting = 0,
Found = 1,
};
BallTracker();
~BallTracker();
bool processTracking();
int processTracking();
void startTracking();
void stopTracking();
@@ -83,6 +90,7 @@ class BallTracker
const double FOV_WIDTH;
const double FOV_HEIGHT;
const int NOT_FOUND_THRESHOLD;
const int WAITING_THRESHOLD;
const bool DEBUG_PRINT;
void ballPositionCallback(const ball_detector::circleSetStamped::ConstPtr &msg);
@@ -107,6 +115,7 @@ class BallTracker
// z is the ball radius
geometry_msgs::Point ball_position_;
int tracking_status_;
bool use_head_scan_;
int count_not_found_;
bool on_tracking_;

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

@@ -42,6 +42,8 @@
#include "op3_demo/ball_tracker.h"
#include "op3_demo/ball_follower.h"
#include "robotis_math/robotis_linear_algebra.h"
#include "op3_action_module_msgs/IsRunning.h"
#include "robotis_controller_msgs/SyncWriteItem.h"
namespace robotis_op
{
@@ -99,26 +101,33 @@ class SoccerDemo : public OPDemo
bool handleFallen(int fallen_status);
void playMotion(int motion_index);
void setRGBLED(int blue, int green, int red);
bool isActionRunning();
BallTracker ball_tracker_;
BallFollower ball_follower_;
ros::Publisher module_control_pub_;
ros::Publisher motion_index_pub_;
ros::Publisher rgb_led_pub_;
ros::Subscriber buttuon_sub_;
ros::Subscriber demo_command_sub_;
ros::Subscriber imu_data_sub_;
ros::ServiceClient is_running_client_;
std::map<int, std::string> id_joint_table_;
std::map<std::string, int> joint_id_table_;
bool is_grass_;
int wait_count_;
bool on_following_ball_;
bool on_following_ball_;
bool restart_soccer_;
bool start_following_;
bool stop_following_;
bool stop_fallen_check_;
int robot_status_;
int tracking_status_;
int stand_state_;
double present_pitch_;
};

4
op3_demo/launch/demo.launch
View File

@@ -1,9 +1,9 @@
<?xml version="1.0"?>
<!-- Launches an UVC camera, the ball detector and its visualization -->
<launch>
<!-- robotis op3 manager -->
<include file="$(find op3_manager)/launch/op3_manager.launch"/>
<include file="$(find op3_manager)/launch/op3_manager.launch"/>
<!-- Camera and Ball detector -->
<include file="$(find ball_detector)/launch/ball_detector_from_usb_cam.launch"/>

14
op3_demo/src/demo_node.cpp
View File

@@ -53,6 +53,7 @@ void goInitPose();
void playSound(const std::string &path);
void setLED(int led);
bool checkManagerRunning(std::string& manager_name);
void dxlTorqueChecker();
const int SPIN_RATE = 30;
const bool DEBUG_PRINT = false;
@@ -60,6 +61,7 @@ const bool DEBUG_PRINT = false;
ros::Publisher init_pose_pub;
ros::Publisher play_sound_pub;
ros::Publisher led_pub;
ros::Publisher dxl_torque_pub;
std::string default_mp3_path = "";
int current_status = Ready;
@@ -83,6 +85,7 @@ int main(int argc, char **argv)
init_pose_pub = nh.advertise<std_msgs::String>("/robotis/base/ini_pose", 0);
play_sound_pub = nh.advertise<std_msgs::String>("/play_sound_file", 0);
led_pub = nh.advertise<robotis_controller_msgs::SyncWriteItem>("/robotis/sync_write_item", 0);
dxl_torque_pub = nh.advertise<std_msgs::String>("/robotis/dxl_torque", 0);
ros::Subscriber buttuon_sub = nh.subscribe("/robotis/open_cr/button", 1, buttonHandlerCallback);
default_mp3_path = ros::package::getPath("op3_demo") + "/Data/mp3/";
@@ -223,14 +226,17 @@ void buttonHandlerCallback(const std_msgs::String::ConstPtr& msg)
switch (desired_status)
{
case SoccerDemo:
dxlTorqueChecker();
playSound(default_mp3_path + "Start soccer demonstration.mp3");
break;
case VisionDemo:
dxlTorqueChecker();
playSound(default_mp3_path + "Start vision processing demonstration.mp3");
break;
case ActionDemo:
dxlTorqueChecker();
playSound(default_mp3_path + "Start motion demonstration.mp3");
break;
@@ -313,3 +319,11 @@ bool checkManagerRunning(std::string& manager_name)
ROS_ERROR("Can't find op3_manager");
return false;
}
void dxlTorqueChecker()
{
std_msgs::String check_msg;
check_msg.data = "check";
dxl_torque_pub.publish(check_msg);
}

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

@@ -46,9 +46,9 @@ BallFollower::BallFollower()
kick_motion_index_(83),
CAMERA_HEIGHT(0.46),
NOT_FOUND_THRESHOLD(50),
MAX_FB_STEP(35.0 * 0.001),
MAX_FB_STEP(40.0 * 0.001),
MAX_RL_TURN(15.0 * M_PI / 180),
MIN_FB_STEP(5.0 * 0.001),
MIN_FB_STEP(0.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),
@@ -177,7 +177,7 @@ void BallFollower::calcFootstep(double target_distance, double target_angle, dou
}
}
// x_angle : ball position (pan), y_angle : ball position (tilt)
// x_angle : ball position (pan), y_angle : ball position (tilt), ball_size : angle of ball radius
bool BallFollower::processFollowing(double x_angle, double y_angle, double ball_size)
{
ros::Time curr_time = ros::Time::now();

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

@@ -40,12 +40,14 @@ BallTracker::BallTracker()
FOV_WIDTH(26.4 * M_PI / 180),
FOV_HEIGHT(21.6 * M_PI / 180),
NOT_FOUND_THRESHOLD(50),
WAITING_THRESHOLD(5),
use_head_scan_(true),
count_not_found_(0),
on_tracking_(false),
current_ball_pan_(0),
current_ball_tilt_(0),
current_ball_bottom_(0),
tracking_status_(NotFound),
DEBUG_PRINT(false)
{
head_joint_pub_ = nh_.advertise<sensor_msgs::JointState>("/robotis/head_control/set_joint_states_offset", 0);
@@ -112,13 +114,15 @@ void BallTracker::setUsingHeadScan(bool use_scan)
use_head_scan_ = use_scan;
}
bool BallTracker::processTracking()
int BallTracker::processTracking()
{
int tracking_status = Found;
if (on_tracking_ == false)
{
ball_position_.z = 0;
count_not_found_ = 0;
return false;
return NotFound;
}
// check ball position
@@ -126,24 +130,56 @@ bool BallTracker::processTracking()
{
count_not_found_++;
if (count_not_found_ > NOT_FOUND_THRESHOLD)
if (count_not_found_ < WAITING_THRESHOLD)
{
if(tracking_status_ == Found || tracking_status_ == Waiting)
tracking_status = Waiting;
else
tracking_status = NotFound;
}
else if (count_not_found_ > NOT_FOUND_THRESHOLD)
{
scanBall();
count_not_found_ = 0;
tracking_status = NotFound;
}
return false;
else
{
tracking_status = NotFound;
}
}
else
{
count_not_found_ = 0;
}
// if ball is found
// convert ball position to desired angle(rad) of head
// ball_position : top-left is (-1, -1), bottom-right is (+1, +1)
// offset_rad : top-left(+, +), bottom-right(-, -)
double x_error = -atan(ball_position_.x * tan(FOV_WIDTH));
double y_error = -atan(ball_position_.y * tan(FOV_HEIGHT));
double x_error = 0.0, y_error = 0.0, ball_size = 0.0;
ball_position_.z = 0;
count_not_found_ = 0;
switch (tracking_status)
{
case NotFound:
tracking_status_ = tracking_status;
return tracking_status;
case Waiting:
x_error = current_ball_pan_ * 0.7;
y_error = current_ball_tilt_ * 0.7;
ball_size = current_ball_bottom_;
break;
case Found:
x_error = -atan(ball_position_.x * tan(FOV_WIDTH));
y_error = -atan(ball_position_.y * tan(FOV_HEIGHT));
ball_size = ball_position_.z;
break;
default:
break;
}
ROS_INFO_STREAM_COND(DEBUG_PRINT, "--------------------------------------------------------------");
ROS_INFO_STREAM_COND(DEBUG_PRINT, "Ball position : " << ball_position_.x << " | " << ball_position_.y);
@@ -154,19 +190,20 @@ bool BallTracker::processTracking()
double delta_time = dur.nsec * 0.000000001 + dur.sec;
prev_time_ = curr_time;
// double p_gain = 0.75, d_gain = 0.05;
double p_gain = 0.7, d_gain = 0.05;
// double p_gain = 0.7, d_gain = 0.05;
double p_gain = 0.75, d_gain = 0.04;
double x_error_diff = (x_error - current_ball_pan_) / delta_time;
double y_error_diff = (y_error - current_ball_tilt_) / delta_time;
double x_error_target = x_error * p_gain + x_error_diff * d_gain;
double y_error_target = y_error * p_gain + y_error_diff * d_gain;
ROS_INFO_STREAM_COND(DEBUG_PRINT, "--------------------------------------------------------------");
ROS_INFO_STREAM_COND(DEBUG_PRINT, "error : " << (x_error * 180 / M_PI) << " | " << (y_error * 180 / M_PI));
ROS_INFO_STREAM_COND(DEBUG_PRINT,
ROS_INFO_STREAM_COND(
DEBUG_PRINT,
"error_diff : " << (x_error_diff * 180 / M_PI) << " | " << (y_error_diff * 180 / M_PI) << " | " << delta_time);
ROS_INFO_STREAM_COND(DEBUG_PRINT,
ROS_INFO_STREAM_COND(
DEBUG_PRINT,
"error_target : " << (x_error_target * 180 / M_PI) << " | " << (y_error_target * 180 / M_PI) << " | P : " << p_gain << " | D : " << d_gain);
// move head joint
@@ -175,9 +212,13 @@ bool BallTracker::processTracking()
// args for following ball
current_ball_pan_ = x_error;
current_ball_tilt_ = y_error;
current_ball_bottom_ = -atan(ball_position_.z * tan(FOV_HEIGHT));
current_ball_bottom_ = ball_size;
return true;
ball_position_.z = 0;
//count_not_found_ = 0;
tracking_status_ = tracking_status;
return tracking_status;
}
void BallTracker::publishHeadJoint(double pan, double tilt)

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

@@ -48,6 +48,7 @@ SoccerDemo::SoccerDemo()
stop_fallen_check_(false),
robot_status_(Waited),
stand_state_(Stand),
tracking_status_(BallTracker::Waiting),
present_pitch_(0)
{
//init ros
@@ -92,14 +93,17 @@ void SoccerDemo::setDemoDisable()
start_following_ = false;
stop_following_ = false;
stop_fallen_check_ = false;
tracking_status_ = BallTracker::Waiting;
}
void SoccerDemo::process()
{
// ball tracking
bool is_tracked;
int tracking_status;
is_tracked = ball_tracker_.processTracking();
tracking_status = ball_tracker_.processTracking();
// check to start
if (start_following_ == true)
@@ -130,10 +134,26 @@ void SoccerDemo::process()
// ball following
if (on_following_ball_ == true)
{
if (is_tracked)
ball_follower_.processFollowing(ball_tracker_.getPanOfBall(), ball_tracker_.getTiltOfBall(), ball_tracker_.getBallSize());
else
ball_follower_.waitFollowing();
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
@@ -171,7 +191,6 @@ void SoccerDemo::process()
{
wait_count_ -= 1;
}
}
void SoccerDemo::processThread()
@@ -201,11 +220,14 @@ void SoccerDemo::callbackThread()
// subscriber & publisher
module_control_pub_ = nh.advertise<robotis_controller_msgs::JointCtrlModule>("/robotis/set_joint_ctrl_modules", 0);
motion_index_pub_ = nh.advertise<std_msgs::Int32>("/robotis/action/page_num", 0);
rgb_led_pub_ = nh.advertise<robotis_controller_msgs::SyncWriteItem>("/robotis/sync_write_item", 0);
buttuon_sub_ = nh.subscribe("/robotis/open_cr/button", 1, &SoccerDemo::buttonHandlerCallback, this);
demo_command_sub_ = nh.subscribe("/ball_tracker/command", 1, &SoccerDemo::demoCommandCallback, this);
imu_data_sub_ = nh.subscribe("/robotis/open_cr/imu", 1, &SoccerDemo::imuDataCallback, this);
is_running_client_ = nh.serviceClient<op3_action_module_msgs::IsRunning>("/robotis/action/is_running");
while (nh.ok())
{
ros::spinOnce();
@@ -405,6 +427,14 @@ void SoccerDemo::imuDataCallback(const sensor_msgs::Imu::ConstPtr& msg)
void SoccerDemo::startSoccerMode()
{
setModuleToDemo("action_module");
usleep(10 * 1000);
playMotion(WalkingReady);
usleep(1500 * 1000);
setBodyModuleToDemo("walking_module");
usleep(10 * 1000);
@@ -491,7 +521,10 @@ bool SoccerDemo::handleFallen(int fallen_status)
break;
}
usleep(500 * 1000);
while(isActionRunning() == true)
usleep(100 * 1000);
usleep(650 * 1000);
if (on_following_ball_ == true)
restart_soccer_ = true;
@@ -510,4 +543,37 @@ void SoccerDemo::playMotion(int motion_index)
motion_index_pub_.publish(motion_msg);
}
void SoccerDemo::setRGBLED(int blue, int green, int red)
{
int led_full_unit = 0x1F;
int led_value = (blue & led_full_unit) << 10 | (green & led_full_unit) << 5 | (red & led_full_unit);
robotis_controller_msgs::SyncWriteItem syncwrite_msg;
syncwrite_msg.item_name = "LED_RGB";
syncwrite_msg.joint_name.push_back("open-cr");
syncwrite_msg.value.push_back(led_value);
rgb_led_pub_.publish(syncwrite_msg);
}
// check running of action
bool SoccerDemo::isActionRunning()
{
op3_action_module_msgs::IsRunning is_running_srv;
if (is_running_client_.call(is_running_srv) == false)
{
ROS_ERROR("Failed to get action status");
return true;
}
else
{
if (is_running_srv.response.is_running == true)
{
return true;
}
}
return false;
}
}

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

@@ -86,16 +86,10 @@ void VisionDemo::setDemoDisable()
void VisionDemo::process()
{
//bool is_tracked = face_tracker_.processTracking();
int tracking_status = face_tracker_.processTracking();
//if(is_tracking_ != is_tracked)
if(tracking_status_ != tracking_status)
{
// if(is_tracked == true)
// setRGBLED(0x1F, 0, 0);
// else
// setRGBLED(0x1F, 0x1F, 0);
switch(tracking_status)
{
case FaceTracker::Found: