commit
70bbf68b56
@ -39,12 +39,12 @@ namespace robotis_op
|
||||
{
|
||||
|
||||
BallDetector::BallDetector()
|
||||
: nh_(ros::this_node::getName()),
|
||||
it_(this->nh_),
|
||||
enable_(true),
|
||||
params_config_(),
|
||||
init_param_(false),
|
||||
not_found_count_(0)
|
||||
: nh_(ros::this_node::getName()),
|
||||
it_(this->nh_),
|
||||
enable_(true),
|
||||
params_config_(),
|
||||
init_param_(false),
|
||||
not_found_count_(0)
|
||||
{
|
||||
has_path_ = nh_.getParam("yaml_path", param_path_);
|
||||
|
||||
@ -127,7 +127,7 @@ bool BallDetector::newImage()
|
||||
|
||||
void BallDetector::process()
|
||||
{
|
||||
if(enable_ == false)
|
||||
if (enable_ == false)
|
||||
return;
|
||||
|
||||
if (cv_img_ptr_sub_ != NULL)
|
||||
@ -145,7 +145,7 @@ void BallDetector::process()
|
||||
|
||||
void BallDetector::publishImage()
|
||||
{
|
||||
if(enable_ == false)
|
||||
if (enable_ == false)
|
||||
return;
|
||||
|
||||
//image_raw topic
|
||||
@ -171,7 +171,7 @@ void BallDetector::publishImage()
|
||||
|
||||
void BallDetector::publishCircles()
|
||||
{
|
||||
if(enable_ == false)
|
||||
if (enable_ == false)
|
||||
return;
|
||||
|
||||
if (circles_.size() == 0)
|
||||
@ -198,10 +198,6 @@ void BallDetector::publishCircles()
|
||||
|
||||
//publish message
|
||||
circles_pub_.publish(circles_msg_);
|
||||
|
||||
//ball_detecting process time
|
||||
//ros::Duration _process_dur = ros::Time::now() - sub_time_;
|
||||
//ROS_INFO_STREAM("== Ball detecting processing time : " << _process_dur);
|
||||
}
|
||||
|
||||
void BallDetector::enableCallback(const std_msgs::Bool::ConstPtr &msg)
|
||||
@ -211,7 +207,7 @@ void BallDetector::enableCallback(const std_msgs::Bool::ConstPtr &msg)
|
||||
|
||||
void BallDetector::imageCallback(const sensor_msgs::ImageConstPtr & msg)
|
||||
{
|
||||
if(enable_ == false)
|
||||
if (enable_ == false)
|
||||
return;
|
||||
|
||||
try
|
||||
@ -272,7 +268,7 @@ void BallDetector::dynParamCallback(ball_detector::detectorParamsConfig &config,
|
||||
|
||||
void BallDetector::cameraInfoCallback(const sensor_msgs::CameraInfo & msg)
|
||||
{
|
||||
if(enable_ == false)
|
||||
if (enable_ == false)
|
||||
return;
|
||||
|
||||
camera_info_msg_ = msg;
|
||||
@ -284,27 +280,27 @@ void BallDetector::printConfig()
|
||||
return;
|
||||
|
||||
std::cout << "Detetctor Configuration:" << std::endl << " gaussian_blur_size: "
|
||||
<< params_config_.gaussian_blur_size << std::endl << " gaussian_blur_sigma: "
|
||||
<< params_config_.gaussian_blur_sigma << std::endl << " canny_edge_th: " << params_config_.canny_edge_th
|
||||
<< std::endl << " hough_accum_resolution: " << params_config_.hough_accum_resolution << std::endl
|
||||
<< " min_circle_dist: " << params_config_.min_circle_dist << std::endl << " hough_accum_th: "
|
||||
<< params_config_.hough_accum_th << std::endl << " min_radius: " << params_config_.min_radius
|
||||
<< std::endl << " max_radius: " << params_config_.max_radius << std::endl << " filter_h_min: "
|
||||
<< params_config_.filter_threshold.h_min << std::endl << " filter_h_max: "
|
||||
<< params_config_.filter_threshold.h_max << std::endl << " filter_s_min: "
|
||||
<< params_config_.filter_threshold.s_min << std::endl << " filter_s_max: "
|
||||
<< params_config_.filter_threshold.s_max << std::endl << " filter_v_min: "
|
||||
<< params_config_.filter_threshold.v_min << std::endl << " filter_v_max: "
|
||||
<< params_config_.filter_threshold.v_max << std::endl << " use_second_filter: "
|
||||
<< params_config_.use_second_filter << std::endl << " filter2_h_min: "
|
||||
<< params_config_.filter2_threshold.h_min << std::endl << " filter2_h_max: "
|
||||
<< params_config_.filter2_threshold.h_max << std::endl << " filter2_s_min: "
|
||||
<< params_config_.filter2_threshold.s_min << std::endl << " filter2_s_max: "
|
||||
<< params_config_.filter2_threshold.s_max << std::endl << " filter2_v_min: "
|
||||
<< params_config_.filter2_threshold.v_min << std::endl << " filter2_v_max: "
|
||||
<< params_config_.filter2_threshold.v_max << std::endl << " ellipse_size: "
|
||||
<< params_config_.ellipse_size << std::endl << " filter_image_to_debug: " << params_config_.debug
|
||||
<< std::endl << std::endl;
|
||||
<< params_config_.gaussian_blur_size << std::endl << " gaussian_blur_sigma: "
|
||||
<< params_config_.gaussian_blur_sigma << std::endl << " canny_edge_th: " << params_config_.canny_edge_th
|
||||
<< std::endl << " hough_accum_resolution: " << params_config_.hough_accum_resolution << std::endl
|
||||
<< " min_circle_dist: " << params_config_.min_circle_dist << std::endl << " hough_accum_th: "
|
||||
<< params_config_.hough_accum_th << std::endl << " min_radius: " << params_config_.min_radius
|
||||
<< std::endl << " max_radius: " << params_config_.max_radius << std::endl << " filter_h_min: "
|
||||
<< params_config_.filter_threshold.h_min << std::endl << " filter_h_max: "
|
||||
<< params_config_.filter_threshold.h_max << std::endl << " filter_s_min: "
|
||||
<< params_config_.filter_threshold.s_min << std::endl << " filter_s_max: "
|
||||
<< params_config_.filter_threshold.s_max << std::endl << " filter_v_min: "
|
||||
<< params_config_.filter_threshold.v_min << std::endl << " filter_v_max: "
|
||||
<< params_config_.filter_threshold.v_max << std::endl << " use_second_filter: "
|
||||
<< params_config_.use_second_filter << std::endl << " filter2_h_min: "
|
||||
<< params_config_.filter2_threshold.h_min << std::endl << " filter2_h_max: "
|
||||
<< params_config_.filter2_threshold.h_max << std::endl << " filter2_s_min: "
|
||||
<< params_config_.filter2_threshold.s_min << std::endl << " filter2_s_max: "
|
||||
<< params_config_.filter2_threshold.s_max << std::endl << " filter2_v_min: "
|
||||
<< params_config_.filter2_threshold.v_min << std::endl << " filter2_v_max: "
|
||||
<< params_config_.filter2_threshold.v_max << std::endl << " ellipse_size: "
|
||||
<< params_config_.ellipse_size << std::endl << " filter_image_to_debug: " << params_config_.debug
|
||||
<< std::endl << std::endl;
|
||||
}
|
||||
|
||||
void BallDetector::saveConfig()
|
||||
@ -364,23 +360,6 @@ void BallDetector::filterImage()
|
||||
if (!in_image_.data)
|
||||
return;
|
||||
|
||||
/*
|
||||
cv::Mat imgCrCb, imgCr;
|
||||
cv::cvtColor(in_image_, imgCrCb, cv::COLOR_BGR2YCrCb);
|
||||
std::vector<cv::Mat> _channels;
|
||||
cv::split(imgCrCb, _channels);
|
||||
imgCr = _channels[2];
|
||||
|
||||
cv::inRange(imgCr, params_config_.filter_h_min, params_config_.filter_h_max, imgCr);
|
||||
cv::erode(imgCr, imgCr, cv::getStructuringElement(cv::MORPH_ELLIPSE, cv::Size(5, 5)) );
|
||||
cv::dilate(imgCr, imgCr, cv::getStructuringElement(cv::MORPH_ELLIPSE, cv::Size(5, 5)) );
|
||||
|
||||
cv::dilate(imgCr, imgCr, cv::getStructuringElement(cv::MORPH_ELLIPSE, cv::Size(5, 5)) );
|
||||
cv::erode(imgCr, imgCr, cv::getStructuringElement(cv::MORPH_ELLIPSE, cv::Size(5, 5)) );
|
||||
|
||||
cv::cvtColor(imgCr, in_image_, cv::COLOR_GRAY2RGB);
|
||||
*/
|
||||
|
||||
cv::Mat img_hsv, img_filtered;
|
||||
cv::cvtColor(in_image_, img_hsv, cv::COLOR_RGB2HSV);
|
||||
|
||||
@ -394,15 +373,6 @@ void BallDetector::filterImage()
|
||||
// mask
|
||||
cv::Mat img_mask;
|
||||
|
||||
// mophology : open and close
|
||||
//int ellipse_size = 30;
|
||||
//int dilate_size = params_config_.max_radius; // dilate_size = ellipse_size * 10
|
||||
//cv::erode(img_filtered, img_mask, cv::getStructuringElement(cv::MORPH_ELLIPSE, cv::Size(ellipse_size, ellipse_size)));
|
||||
//cv::dilate(img_mask, img_mask,
|
||||
// cv::getStructuringElement(cv::MORPH_ELLIPSE, cv::Size(dilate_size, dilate_size)));
|
||||
|
||||
//makeFilterMask(img_filtered, img_mask, ellipse_size);
|
||||
|
||||
// check hsv range
|
||||
cv::Mat img_filtered2;
|
||||
inRangeHsv(img_hsv, params_config_.filter2_threshold, img_filtered2);
|
||||
@ -410,8 +380,6 @@ void BallDetector::filterImage()
|
||||
makeFilterMaskFromBall(img_filtered, img_mask);
|
||||
cv::bitwise_and(img_filtered2, img_mask, img_filtered2);
|
||||
|
||||
// mophology(img_filtered2, img_filtered2, params_config_.ellipse_size);
|
||||
|
||||
// or
|
||||
cv::bitwise_or(img_filtered, img_filtered2, img_filtered);
|
||||
}
|
||||
@ -437,7 +405,7 @@ void BallDetector::makeFilterMask(const cv::Mat &source_img, cv::Mat &mask_img,
|
||||
{
|
||||
for (int j = 0; j < source_width; j++)
|
||||
{
|
||||
uint8_t pixel = source_img.at<uint8_t>(i, j);
|
||||
uint8_t pixel = source_img.at < uint8_t > (i, j);
|
||||
|
||||
if (pixel == 0)
|
||||
continue;
|
||||
@ -452,7 +420,7 @@ void BallDetector::makeFilterMask(const cv::Mat &source_img, cv::Mat &mask_img,
|
||||
if (mask_j < 0 || mask_j >= source_width)
|
||||
continue;
|
||||
|
||||
mask_img.at<uchar>(mask_i, mask_j, 0) = 255;
|
||||
mask_img.at < uchar > (mask_i, mask_j, 0) = 255;
|
||||
}
|
||||
}
|
||||
}
|
||||
@ -464,51 +432,22 @@ void BallDetector::makeFilterMaskFromBall(const cv::Mat &source_img, cv::Mat &ma
|
||||
// source_img.
|
||||
mask_img = cv::Mat::zeros(source_img.rows, source_img.cols, CV_8UC1);
|
||||
|
||||
if(circles_.size() == 0)
|
||||
if (circles_.size() == 0)
|
||||
return;
|
||||
|
||||
// channel : 1
|
||||
if (source_img.channels() != 1)
|
||||
return;
|
||||
|
||||
// for (int i = 0; i < circles_.size(); i++)
|
||||
// {
|
||||
// if (circles_[i][0] != -1)
|
||||
// {
|
||||
// // _center = cv::Point(cvRound(circles_[_i][0]), cvRound(circles_[_i][1]));
|
||||
// int center_x = cvRound(circles_[i][0]);
|
||||
// int center_y = cvRound(circles_[i][1]);
|
||||
// int radius = cvRound(circles_[i][2]) * 1.5;
|
||||
//
|
||||
// for (int mask_i = center_y - radius; mask_i <= center_y + radius; mask_i++)
|
||||
// {
|
||||
// if (mask_i < 0 || mask_i >= source_img.rows)
|
||||
// continue;
|
||||
//
|
||||
// int mask_offset = abs(mask_i - center_y) * 0.5;
|
||||
//
|
||||
// for (int mask_j = center_x - radius + mask_offset; mask_j <= center_x + radius - mask_offset; mask_j++)
|
||||
// {
|
||||
// if (mask_j < 0 || mask_j >= source_img.cols)
|
||||
// continue;
|
||||
//
|
||||
// mask_img.at<uchar>(mask_i, mask_j, 0) = 255;
|
||||
// }
|
||||
// }
|
||||
//
|
||||
// }
|
||||
// }
|
||||
|
||||
cv::Mat img_labels, stats, centroids;
|
||||
int numOfLables = cv::connectedComponentsWithStats(source_img, img_labels,
|
||||
stats, centroids, 8,CV_32S);
|
||||
int numOfLables = cv::connectedComponentsWithStats(source_img, img_labels, stats, centroids, 8, CV_32S);
|
||||
for (int j = 1; j < numOfLables; j++)
|
||||
{
|
||||
int area = stats.at<int>(j, cv::CC_STAT_AREA);
|
||||
int left = stats.at<int>(j, cv::CC_STAT_LEFT);
|
||||
int top = stats.at<int>(j, cv::CC_STAT_TOP);
|
||||
int top = stats.at<int>(j, cv::CC_STAT_TOP);
|
||||
int width = stats.at<int>(j, cv::CC_STAT_WIDTH);
|
||||
int height = stats.at<int>(j, cv::CC_STAT_HEIGHT);
|
||||
int height = stats.at<int>(j, cv::CC_STAT_HEIGHT);
|
||||
|
||||
int center_x = left + width * 0.5;
|
||||
int center_y = top + height * 0.5;
|
||||
@ -526,7 +465,7 @@ void BallDetector::makeFilterMaskFromBall(const cv::Mat &source_img, cv::Mat &ma
|
||||
if (mask_j < 0 || mask_j >= source_img.cols)
|
||||
continue;
|
||||
|
||||
mask_img.at<uchar>(mask_i, mask_j, 0) = 255;
|
||||
mask_img.at < uchar > (mask_i, mask_j, 0) = 255;
|
||||
}
|
||||
}
|
||||
}
|
||||
@ -588,40 +527,31 @@ void BallDetector::houghDetection(const unsigned int imgEncoding)
|
||||
cv::cvtColor(in_image_, gray_image, CV_RGB2GRAY);
|
||||
|
||||
//Reduce the noise so we avoid false circle detection
|
||||
cv::GaussianBlur(gray_image, gray_image, cv::Size(params_config_.gaussian_blur_size, params_config_.gaussian_blur_size),
|
||||
cv::GaussianBlur(gray_image, gray_image,
|
||||
cv::Size(params_config_.gaussian_blur_size, params_config_.gaussian_blur_size),
|
||||
params_config_.gaussian_blur_sigma);
|
||||
|
||||
//double hough_accum_th = (not_found_count_ < NOT_FOUND_TH) ?
|
||||
// params_config_.hough_accum_th : params_config_.hough_accum_th * 0.5;
|
||||
double hough_accum_th = params_config_.hough_accum_th;
|
||||
|
||||
// std::cout << "Not found : " << not_found_count_ << ", value : " << hough_accum_th << std::endl;
|
||||
|
||||
//Apply the Hough Transform to find the circles
|
||||
cv::HoughCircles(gray_image, circles_current, CV_HOUGH_GRADIENT, params_config_.hough_accum_resolution,
|
||||
params_config_.min_circle_dist, params_config_.canny_edge_th, hough_accum_th,
|
||||
params_config_.min_radius, params_config_.max_radius);
|
||||
|
||||
//set found circles to circles set. Apply some condition if desired.
|
||||
//for (int ii = 0; ii < circles_current.size(); ii++)
|
||||
//{
|
||||
// circles_.push_back(circles_current.at(ii));
|
||||
// // std::cout << "circle " << ii << ": (" << circles.at(ii)[0] << "," << circles.at(ii)[1] << ")" << std::endl;
|
||||
//}
|
||||
|
||||
if(circles_current.size() == 0)
|
||||
if (circles_current.size() == 0)
|
||||
not_found_count_ += 1;
|
||||
else
|
||||
not_found_count_ = 0;
|
||||
|
||||
double alpha = 0.2;
|
||||
|
||||
for(int ix = 0; ix < circles_current.size(); ix++)
|
||||
for (int ix = 0; ix < circles_current.size(); ix++)
|
||||
{
|
||||
cv::Point2d center = cv::Point(circles_current[ix][0], circles_current[ix][1]);
|
||||
double radius = circles_current[ix][2];
|
||||
|
||||
for(int prev_ix = 0; prev_ix < prev_circles.size(); prev_ix++)
|
||||
for (int prev_ix = 0; prev_ix < prev_circles.size(); prev_ix++)
|
||||
{
|
||||
cv::Point2d prev_center = cv::Point(prev_circles[prev_ix][0], prev_circles[prev_ix][1]);
|
||||
double prev_radius = prev_circles[prev_ix][2];
|
||||
@ -629,13 +559,11 @@ void BallDetector::houghDetection(const unsigned int imgEncoding)
|
||||
cv::Point2d diff = center - prev_center;
|
||||
double radius_th = std::max(radius, prev_radius) * 0.75;
|
||||
|
||||
if(sqrt(diff.dot(diff)) < radius_th)
|
||||
if (sqrt(diff.dot(diff)) < radius_th)
|
||||
{
|
||||
if(abs(radius - prev_radius) < radius_th)
|
||||
if (abs(radius - prev_radius) < radius_th)
|
||||
{
|
||||
// std::cout << "circle - raw " << ix << ": (" << circles_current.at(ix)[0] << "," << circles_current.at(ix)[1] << ")" << std::endl;
|
||||
circles_current[ix] = circles_current[ix] * alpha + prev_circles[prev_ix] * (1 - alpha);
|
||||
// std::cout << "circle - lpf " << ix << ": (" << circles_current.at(ix)[0] << "," << circles_current.at(ix)[1] << ")" << std::endl;
|
||||
}
|
||||
|
||||
break;
|
||||
@ -659,11 +587,6 @@ void BallDetector::drawOutputImage()
|
||||
for (ii = 0; ii < circles_.size(); ii++)
|
||||
{
|
||||
{
|
||||
// cv::Point _center = cv::Point(cvRound(circles_[ii][0]), cvRound(circles_[ii][1]));
|
||||
// int _radius = cvRound(circles_[ii][2]);
|
||||
// cv::circle( out_image_, _center, 5, cv::Scalar(0,0,255), -1, 8, 0 );// circle center in green
|
||||
// cv::circle( out_image_, _center, _radius, cv::Scalar(0,0,255), 3, 8, 0 );// circle outline in red
|
||||
|
||||
int this_radius = cvRound(circles_[ii][2]);
|
||||
if (this_radius > radius)
|
||||
{
|
||||
|
@ -45,31 +45,6 @@ include_directories(
|
||||
${Eigen_INCLUDE_DIRS}
|
||||
)
|
||||
|
||||
add_executable(ball_tracker_node
|
||||
src/ball_tracker_node.cpp
|
||||
src/soccer/ball_tracker.cpp
|
||||
)
|
||||
|
||||
add_dependencies(ball_tracker_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
|
||||
|
||||
target_link_libraries(ball_tracker_node
|
||||
${catkin_LIBRARIES}
|
||||
yaml-cpp
|
||||
)
|
||||
|
||||
add_executable(soccer_demo_node
|
||||
src/soccer_demo_node.cpp
|
||||
src/soccer/ball_tracker.cpp
|
||||
src/soccer/ball_follower.cpp
|
||||
)
|
||||
|
||||
add_dependencies(soccer_demo_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
|
||||
|
||||
target_link_libraries(soccer_demo_node
|
||||
${catkin_LIBRARIES}
|
||||
yaml-cpp
|
||||
)
|
||||
|
||||
add_executable(op_demo_node
|
||||
src/demo_node.cpp
|
||||
src/soccer/soccer_demo.cpp
|
||||
|
@ -95,8 +95,9 @@ class BallFollower
|
||||
void setWalkingCommand(const std::string &command);
|
||||
void setWalkingParam(double x_move, double y_move, double rotation_angle, bool balance = true);
|
||||
bool getWalkingParam();
|
||||
|
||||
|
||||
void calcFootstep(double target_distance, double target_angle, double delta_time,
|
||||
double& fb_move, double& rl_angle);
|
||||
|
||||
//ros node handle
|
||||
ros::NodeHandle nh_;
|
||||
|
||||
@ -130,6 +131,9 @@ class BallFollower
|
||||
double hip_pitch_offset_;
|
||||
ros::Time prev_time_;
|
||||
|
||||
double curr_period_time_;
|
||||
double accum_period_time_;
|
||||
|
||||
};
|
||||
}
|
||||
|
||||
|
@ -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_;
|
||||
|
@ -48,6 +48,7 @@ class OPDemo
|
||||
RightKick = 121,
|
||||
LeftKick = 120,
|
||||
Ceremony = 85,
|
||||
ForGrass = 20,
|
||||
};
|
||||
|
||||
OPDemo()
|
||||
|
@ -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,25 +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_;
|
||||
};
|
||||
|
@ -1,10 +0,0 @@
|
||||
<?xml version="1.0"?>
|
||||
<!-- Launches an UVC camera, the ball detector and its visualization -->
|
||||
<launch>
|
||||
<!-- Camera and Ball detector -->
|
||||
<include file="$(find ball_detector)/launch/ball_detector_from_usb_cam.launch"/>
|
||||
|
||||
<!-- <node pkg="ball_tracker" type="ball_tracker_node" name="ball_tracker" output="screen"/> -->
|
||||
<node pkg="op3_demo" type="ball_tracker_node" name="ball_tracker" output="screen"/>
|
||||
</launch>
|
||||
|
@ -1,9 +1,9 @@
|
||||
<?xml version="1.0"?>
|
||||
<!-- Launches an UVC camera, the ball detector and its visualization -->
|
||||
|
||||
<launch>
|
||||
|
||||
<!-- op3 manager -->
|
||||
<include file="$(find op3_manager)/launch/op3_manager.launch"/>
|
||||
<!-- robotis op3 manager -->
|
||||
<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"/>
|
||||
@ -17,7 +17,9 @@
|
||||
<!-- sound player -->
|
||||
<node pkg="ros_madplay_player" type="ros_madplay_player" name="ros_madplay_player" output="screen"/>
|
||||
|
||||
<!-- op3 demo -->
|
||||
<node pkg="op3_demo" type="op_demo_node" name="op3_demo" output="screen"/>
|
||||
<!-- robotis op3 demo -->
|
||||
<node pkg="op3_demo" type="op_demo_node" name="op3_demo" output="screen">
|
||||
<param name="grass_demo" type="bool" value="True" />
|
||||
</node>
|
||||
</launch>
|
||||
|
||||
|
@ -2,7 +2,7 @@
|
||||
<!-- Launches an UVC camera, the ball detector and its visualization -->
|
||||
<launch>
|
||||
|
||||
<!-- op3 manager -->
|
||||
<!-- robotis op3 manager -->
|
||||
<include file="$(find op3_manager)/launch/op3_manager.launch"/>
|
||||
|
||||
<!-- Camera and Ball detector -->
|
||||
@ -17,7 +17,7 @@
|
||||
<!-- sound player -->
|
||||
<node pkg="ros_madplay_player" type="ros_madplay_player" name="ros_madplay_player" output="screen"/>
|
||||
|
||||
<!-- op3 demo -->
|
||||
<!-- robotis op3 self test demo -->
|
||||
<node pkg="op3_demo" type="self_test_node" name="op3_self_test" output="screen"/>
|
||||
</launch>
|
||||
|
||||
|
@ -1,133 +0,0 @@
|
||||
/*******************************************************************************
|
||||
* Copyright (c) 2016, ROBOTIS CO., LTD.
|
||||
* All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions are met:
|
||||
*
|
||||
* * Redistributions of source code must retain the above copyright notice, this
|
||||
* list of conditions and the following disclaimer.
|
||||
*
|
||||
* * Redistributions in binary form must reproduce the above copyright notice,
|
||||
* this list of conditions and the following disclaimer in the documentation
|
||||
* and/or other materials provided with the distribution.
|
||||
*
|
||||
* * Neither the name of ROBOTIS nor the names of its
|
||||
* contributors may be used to endorse or promote products derived from
|
||||
* this software without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
||||
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
||||
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
|
||||
* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
|
||||
* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
|
||||
* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
|
||||
* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
||||
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
|
||||
* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
|
||||
* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
*******************************************************************************/
|
||||
|
||||
/* Author: Kayman Jung */
|
||||
|
||||
#include "op3_demo/ball_tracker.h"
|
||||
|
||||
enum Demo_Status
|
||||
{
|
||||
Ready = 0,
|
||||
DesireToTrack = 1,
|
||||
DesireToStop = 2,
|
||||
Tracking = 3,
|
||||
DemoCount = 4,
|
||||
};
|
||||
int current_status = Ready;
|
||||
|
||||
void buttonHandlerCallback(const std_msgs::String::ConstPtr& msg);
|
||||
|
||||
|
||||
//node main
|
||||
int main(int argc, char **argv)
|
||||
{
|
||||
//init ros
|
||||
ros::init(argc, argv, "ball_tracker_node");
|
||||
ros::NodeHandle nh("~");
|
||||
ros::Publisher module_control_pub_ = nh.advertise<std_msgs::String>("/robotis/enable_ctrl_module", 0);
|
||||
ros::Subscriber buttuon_sub = nh.subscribe("/robotis/open_cr/button", 1, buttonHandlerCallback);
|
||||
|
||||
//create ros wrapper object
|
||||
robotis_op::BallTracker tracker;
|
||||
|
||||
// set head_control_module
|
||||
std_msgs::String control_msg;
|
||||
control_msg.data = "head_control_module";
|
||||
|
||||
usleep(1000 * 1000);
|
||||
|
||||
module_control_pub_.publish(control_msg);
|
||||
|
||||
// start ball tracking
|
||||
tracker.startTracking();
|
||||
current_status = Tracking;
|
||||
|
||||
//set node loop rate
|
||||
ros::Rate loop_rate(30);
|
||||
|
||||
//node loop
|
||||
while (ros::ok())
|
||||
{
|
||||
switch (current_status)
|
||||
{
|
||||
case DesireToTrack:
|
||||
tracker.startTracking();
|
||||
current_status = Tracking;
|
||||
break;
|
||||
|
||||
case DesireToStop:
|
||||
tracker.stopTracking();
|
||||
current_status = Ready;
|
||||
break;
|
||||
|
||||
case Tracking:
|
||||
tracker.processTracking();
|
||||
break;
|
||||
|
||||
default:
|
||||
break;
|
||||
}
|
||||
|
||||
//execute pending callback
|
||||
ros::spinOnce();
|
||||
|
||||
//relax to fit output rate
|
||||
loop_rate.sleep();
|
||||
}
|
||||
|
||||
//exit program
|
||||
return 0;
|
||||
}
|
||||
|
||||
void buttonHandlerCallback(const std_msgs::String::ConstPtr& msg)
|
||||
{
|
||||
if (msg->data == "mode_long")
|
||||
{
|
||||
|
||||
}
|
||||
else if (msg->data == "start_long")
|
||||
{
|
||||
// it's using in op3_manager
|
||||
// torque on and going to init pose
|
||||
}
|
||||
|
||||
if (msg->data == "start")
|
||||
{
|
||||
if (current_status == Ready)
|
||||
current_status = DesireToTrack;
|
||||
else if (current_status == Tracking)
|
||||
current_status = DesireToStop;
|
||||
}
|
||||
else if (msg->data == "mode")
|
||||
{
|
||||
|
||||
}
|
||||
|
||||
}
|
@ -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/";
|
||||
@ -189,6 +192,9 @@ int main(int argc, char **argv)
|
||||
|
||||
void buttonHandlerCallback(const std_msgs::String::ConstPtr& msg)
|
||||
{
|
||||
if(apply_desired == true)
|
||||
return;
|
||||
|
||||
// in the middle of playing demo
|
||||
if (current_status != Ready)
|
||||
{
|
||||
@ -220,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;
|
||||
|
||||
@ -310,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);
|
||||
}
|
||||
|
@ -46,20 +46,22 @@ 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(0.5 * 0.001),
|
||||
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 * M_PI / 180),
|
||||
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,
|
||||
@ -90,10 +92,12 @@ void BallFollower::startFollowing()
|
||||
if (result == true)
|
||||
{
|
||||
hip_pitch_offset_ = current_walking_param_.hip_pitch_offset;
|
||||
curr_period_time_ = current_walking_param_.period_time;
|
||||
}
|
||||
else
|
||||
{
|
||||
hip_pitch_offset_ = 7.0 * M_PI / 180;
|
||||
curr_period_time_ = 0.6;
|
||||
}
|
||||
}
|
||||
|
||||
@ -135,7 +139,45 @@ void BallFollower::currentJointStatesCallback(const sensor_msgs::JointState::Con
|
||||
current_tilt_ = tilt;
|
||||
}
|
||||
|
||||
// x_angle : ball position (pan), y_angle : ball position (tilt)
|
||||
void BallFollower::calcFootstep(double target_distance, double target_angle, double delta_time,
|
||||
double& fb_move, double& rl_angle)
|
||||
{
|
||||
// clac fb
|
||||
double next_movement = current_x_move_;
|
||||
if (target_distance < 0)
|
||||
target_distance = 0.0;
|
||||
|
||||
double fb_goal = fmin(target_distance * 0.1, MAX_FB_STEP);
|
||||
accum_period_time_ += delta_time;
|
||||
if (accum_period_time_ > (curr_period_time_ / 4))
|
||||
{
|
||||
accum_period_time_ = 0.0;
|
||||
if ((target_distance * 0.1 / 2) < current_x_move_)
|
||||
next_movement -= UNIT_FB_STEP;
|
||||
else
|
||||
next_movement += UNIT_FB_STEP;
|
||||
}
|
||||
fb_goal = fmin(next_movement, fb_goal);
|
||||
fb_move = fmax(fb_goal, MIN_FB_STEP);
|
||||
ROS_INFO_COND(DEBUG_PRINT, "distance to ball : %6.4f, fb : %6.4f, delta : %6.6f", target_distance, fb_move,
|
||||
delta_time);
|
||||
ROS_INFO_COND(DEBUG_PRINT, "==============================================");
|
||||
|
||||
// calc rl angle
|
||||
double rl_goal = 0.0;
|
||||
if (fabs(target_angle) * 180 / M_PI > 5.0)
|
||||
{
|
||||
double rl_offset = fabs(target_angle) * 0.2;
|
||||
rl_goal = fmin(rl_offset, MAX_RL_TURN);
|
||||
rl_goal = fmax(rl_goal, MIN_RL_TURN);
|
||||
rl_angle = fmin(fabs(current_r_angle_) + UNIT_RL_TURN, rl_goal);
|
||||
|
||||
if (target_angle < 0)
|
||||
rl_angle *= (-1);
|
||||
}
|
||||
}
|
||||
|
||||
// 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();
|
||||
@ -165,7 +207,6 @@ bool BallFollower::processFollowing(double x_angle, double y_angle, double ball_
|
||||
|
||||
approach_ball_position_ = NotFound;
|
||||
|
||||
// clac fb
|
||||
double distance_to_ball = CAMERA_HEIGHT * tan(M_PI * 0.5 + current_tilt_ - hip_pitch_offset_ - ball_size);
|
||||
|
||||
double ball_y_angle = (current_tilt_ + y_angle) * 180 / M_PI;
|
||||
@ -174,8 +215,8 @@ bool BallFollower::processFollowing(double x_angle, double y_angle, double ball_
|
||||
if (distance_to_ball < 0)
|
||||
distance_to_ball *= (-1);
|
||||
|
||||
double fb_goal, fb_move;
|
||||
double distance_to_kick = 0.25;
|
||||
//double distance_to_kick = 0.25;
|
||||
double distance_to_kick = 0.22;
|
||||
|
||||
// check whether ball is correct position.
|
||||
if ((distance_to_ball < distance_to_kick) && (fabs(ball_x_angle) < 25.0))
|
||||
@ -228,38 +269,17 @@ bool BallFollower::processFollowing(double x_angle, double y_angle, double ball_
|
||||
accum_ball_position_ = 0;
|
||||
}
|
||||
|
||||
fb_goal = fmin(distance_to_ball * 0.1, MAX_FB_STEP);
|
||||
if ((distance_to_ball * 0.1 / 2) < current_x_move_)
|
||||
{
|
||||
fb_goal = fmin(current_x_move_ - UNIT_FB_STEP, fb_goal);
|
||||
fb_move = fmax(fb_goal, MIN_FB_STEP);
|
||||
}
|
||||
else
|
||||
{
|
||||
fb_goal = fmin(current_x_move_ + UNIT_FB_STEP, fb_goal);
|
||||
fb_move = fmax(fb_goal, MIN_FB_STEP);
|
||||
}
|
||||
double fb_move = 0.0, rl_angle = 0.0;
|
||||
double distance_to_walk = distance_to_ball - distance_to_kick;
|
||||
|
||||
ROS_INFO_COND(DEBUG_PRINT, "distance to ball : %6.4f, fb : %6.4f, delta : %6.6f", distance_to_ball, fb_move,
|
||||
delta_time);
|
||||
ROS_INFO_COND(DEBUG_PRINT, "==============================================");
|
||||
|
||||
// calc rl angle
|
||||
double rl_goal, rl_angle;
|
||||
if (fabs(current_pan_) * 180 / M_PI > 5.0)
|
||||
{
|
||||
double rl_offset = fabs(current_pan_) * 0.3;
|
||||
rl_goal = fmin(rl_offset, MAX_RL_TURN);
|
||||
rl_goal = fmax(rl_goal, MIN_RL_TURN);
|
||||
rl_angle = fmin(fabs(current_r_angle_) + UNIT_RL_TURN, rl_goal);
|
||||
|
||||
if (current_pan_ < 0)
|
||||
rl_angle *= (-1);
|
||||
}
|
||||
calcFootstep(distance_to_walk, current_pan_, delta_time, fb_move, rl_angle);
|
||||
|
||||
// send message
|
||||
setWalkingParam(fb_move, 0, rl_angle);
|
||||
|
||||
// for debug
|
||||
//ROS_INFO("distance to ball : %6.4f, fb : %6.4f, delta : %6.6f", distance_to_ball, fb_move, delta_time);
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
|
@ -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);
|
||||
@ -101,6 +103,10 @@ void BallTracker::stopTracking()
|
||||
{
|
||||
on_tracking_ = false;
|
||||
ROS_INFO_COND(DEBUG_PRINT, "Stop Ball tracking");
|
||||
|
||||
double x_error = -atan(ball_position_.x * tan(FOV_WIDTH));
|
||||
double y_error = -atan(ball_position_.y * tan(FOV_HEIGHT));
|
||||
publishHeadJoint(x_error, y_error);
|
||||
}
|
||||
|
||||
void BallTracker::setUsingHeadScan(bool use_scan)
|
||||
@ -108,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
|
||||
@ -122,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);
|
||||
@ -150,18 +190,20 @@ bool BallTracker::processTracking()
|
||||
double delta_time = dur.nsec * 0.000000001 + dur.sec;
|
||||
prev_time_ = curr_time;
|
||||
|
||||
double p_gain = 0.7, d_gain = 0.02;
|
||||
// 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
|
||||
@ -170,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)
|
||||
|
@ -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
|
||||
@ -61,6 +62,8 @@ SoccerDemo::SoccerDemo()
|
||||
|
||||
boost::thread queue_thread = boost::thread(boost::bind(&SoccerDemo::callbackThread, this));
|
||||
boost::thread process_thread = boost::thread(boost::bind(&SoccerDemo::processThread, this));
|
||||
|
||||
is_grass_ = nh.param<bool>("grass_demo", false);
|
||||
}
|
||||
|
||||
SoccerDemo::~SoccerDemo()
|
||||
@ -90,25 +93,32 @@ 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)
|
||||
{
|
||||
ball_tracker_.startTracking();
|
||||
// for debug
|
||||
ball_follower_.startFollowing();
|
||||
start_following_ = false;
|
||||
|
||||
wait_count_ = 1 * SPIN_RATE;
|
||||
}
|
||||
|
||||
//for debug
|
||||
//return;
|
||||
|
||||
// check to stop
|
||||
if (stop_following_ == true)
|
||||
{
|
||||
@ -124,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
|
||||
@ -165,7 +191,6 @@ void SoccerDemo::process()
|
||||
{
|
||||
wait_count_ -= 1;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
void SoccerDemo::processThread()
|
||||
@ -195,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();
|
||||
@ -399,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);
|
||||
@ -420,8 +456,7 @@ void SoccerDemo::handleKick(int ball_position)
|
||||
usleep(1000 * 1000);
|
||||
|
||||
// change to motion module
|
||||
//setModuleToDemo("action_module");
|
||||
setBodyModuleToDemo("action_module");
|
||||
setModuleToDemo("action_module");
|
||||
|
||||
usleep(1500 * 1000);
|
||||
|
||||
@ -433,12 +468,12 @@ void SoccerDemo::handleKick(int ball_position)
|
||||
{
|
||||
case robotis_op::BallFollower::OnRight:
|
||||
std::cout << "Kick Motion [R]: " << ball_position << std::endl;
|
||||
playMotion(RightKick);
|
||||
playMotion(is_grass_ ? RightKick + ForGrass : RightKick);
|
||||
break;
|
||||
|
||||
case robotis_op::BallFollower::OnLeft:
|
||||
std::cout << "Kick Motion [L]: " << ball_position << std::endl;
|
||||
playMotion(LeftKick);
|
||||
playMotion(is_grass_ ? LeftKick + ForGrass : LeftKick);
|
||||
break;
|
||||
|
||||
default:
|
||||
@ -446,6 +481,7 @@ void SoccerDemo::handleKick(int ball_position)
|
||||
}
|
||||
|
||||
on_following_ball_ = false;
|
||||
restart_soccer_ = true;
|
||||
|
||||
usleep(2000 * 1000);
|
||||
|
||||
@ -454,8 +490,6 @@ void SoccerDemo::handleKick(int ball_position)
|
||||
|
||||
// ceremony
|
||||
//playMotion(Ceremony);
|
||||
|
||||
restart_soccer_ = true;
|
||||
}
|
||||
|
||||
bool SoccerDemo::handleFallen(int fallen_status)
|
||||
@ -475,19 +509,22 @@ bool SoccerDemo::handleFallen(int fallen_status)
|
||||
{
|
||||
case Fallen_Forward:
|
||||
std::cout << "Getup Motion [F]: " << std::endl;
|
||||
playMotion(GetUpFront);
|
||||
playMotion(is_grass_ ? GetUpFront + ForGrass : GetUpFront);
|
||||
break;
|
||||
|
||||
case Fallen_Behind:
|
||||
std::cout << "Getup Motion [B]: " << std::endl;
|
||||
playMotion(GetUpBack);
|
||||
playMotion(is_grass_ ? GetUpBack + ForGrass : GetUpBack);
|
||||
break;
|
||||
|
||||
default:
|
||||
break;
|
||||
}
|
||||
|
||||
usleep(500 * 1000);
|
||||
while(isActionRunning() == true)
|
||||
usleep(100 * 1000);
|
||||
|
||||
usleep(650 * 1000);
|
||||
|
||||
if (on_following_ball_ == true)
|
||||
restart_soccer_ = true;
|
||||
@ -506,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;
|
||||
}
|
||||
|
||||
}
|
||||
|
@ -1,504 +0,0 @@
|
||||
/*******************************************************************************
|
||||
* Copyright (c) 2016, ROBOTIS CO., LTD.
|
||||
* All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions are met:
|
||||
*
|
||||
* * Redistributions of source code must retain the above copyright notice, this
|
||||
* list of conditions and the following disclaimer.
|
||||
*
|
||||
* * Redistributions in binary form must reproduce the above copyright notice,
|
||||
* this list of conditions and the following disclaimer in the documentation
|
||||
* and/or other materials provided with the distribution.
|
||||
*
|
||||
* * Neither the name of ROBOTIS nor the names of its
|
||||
* contributors may be used to endorse or promote products derived from
|
||||
* this software without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
||||
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
||||
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
|
||||
* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
|
||||
* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
|
||||
* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
|
||||
* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
||||
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
|
||||
* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
|
||||
* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
*******************************************************************************/
|
||||
|
||||
/* Author: Kayman Jung */
|
||||
|
||||
#include <ros/ros.h>
|
||||
#include <std_msgs/String.h>
|
||||
#include <sensor_msgs/Imu.h>
|
||||
#include <boost/thread.hpp>
|
||||
|
||||
#include "op3_demo/ball_tracker.h"
|
||||
#include "op3_demo/ball_follower.h"
|
||||
#include "robotis_math/robotis_linear_algebra.h"
|
||||
|
||||
enum Motion_Index
|
||||
{
|
||||
InitPose = 1,
|
||||
WalkingReady = 9,
|
||||
GetUpFront = 122,
|
||||
GetUpBack = 123,
|
||||
RightKick = 121,
|
||||
LeftKick = 120,
|
||||
Ceremony = 85,
|
||||
};
|
||||
|
||||
enum Stand_Status
|
||||
{
|
||||
Stand = 0,
|
||||
Fallen_Forward = 1,
|
||||
Fallen_Behind = 2,
|
||||
};
|
||||
|
||||
enum Robot_Status
|
||||
{
|
||||
Waited = 0,
|
||||
TrackingAndFollowing = 1,
|
||||
ReadyToKick = 2,
|
||||
ReadyToCeremony = 3,
|
||||
ReadyToGetup = 4,
|
||||
};
|
||||
|
||||
const double FALL_FORWARD_LIMIT = 60;
|
||||
const double FALL_BACK_LIMIT = -60;
|
||||
const int SPIN_RATE = 30;
|
||||
|
||||
void callbackThread();
|
||||
|
||||
void setBodyModuleToDemo(const std::string &body_module);
|
||||
void setModuleToDemo(const std::string &module_name);
|
||||
void parseJointNameFromYaml(const std::string &path);
|
||||
bool getJointNameFromID(const int &id, std::string &joint_name);
|
||||
bool getIDFromJointName(const std::string &joint_name, int &id);
|
||||
void buttonHandlerCallback(const std_msgs::String::ConstPtr& msg);
|
||||
void demoCommandCallback(const std_msgs::String::ConstPtr& msg);
|
||||
void imuDataCallback(const sensor_msgs::Imu::ConstPtr& msg);
|
||||
|
||||
void startSoccerMode();
|
||||
void stopSoccerMode();
|
||||
|
||||
void handleKick(int ball_position);
|
||||
bool handleFallen(int fallen_status);
|
||||
|
||||
void playMotion(int motion_index);
|
||||
|
||||
ros::Publisher module_control_pub;
|
||||
ros::Publisher motion_index_pub;
|
||||
ros::Subscriber buttuon_sub;
|
||||
ros::Subscriber demo_command_sub;
|
||||
ros::Subscriber imu_data_sub;
|
||||
std::map<int, std::string> id_joint_table;
|
||||
std::map<std::string, int> joint_id_table;
|
||||
|
||||
int wait_count = 0;
|
||||
bool on_following_ball = false;
|
||||
bool restart_soccer = false;
|
||||
bool start_following = false;
|
||||
bool stop_following = false;
|
||||
bool stop_fallen_check = false;
|
||||
int robot_status = Waited;
|
||||
int stand_state = Stand;
|
||||
double present_pitch = 0;
|
||||
|
||||
bool debug_code = false;
|
||||
|
||||
//node main
|
||||
int main(int argc, char **argv)
|
||||
{
|
||||
//init ros
|
||||
ros::init(argc, argv, "soccer_demo_node");
|
||||
|
||||
//create ros wrapper object
|
||||
robotis_op::BallTracker tracker;
|
||||
robotis_op::BallFollower follower;
|
||||
|
||||
ros::NodeHandle nh(ros::this_node::getName());
|
||||
|
||||
std::string _default_path = ros::package::getPath("op3_gui_demo") + "/config/demo_config.yaml";
|
||||
std::string _path = nh.param<std::string>("demo_config", _default_path);
|
||||
parseJointNameFromYaml(_path);
|
||||
|
||||
boost::thread queue_thread = boost::thread(boost::bind(&callbackThread));
|
||||
|
||||
bool result = false;
|
||||
|
||||
//set node loop rate
|
||||
ros::Rate loop_rate(SPIN_RATE);
|
||||
|
||||
tracker.startTracking();
|
||||
|
||||
//node loop
|
||||
while (ros::ok())
|
||||
{
|
||||
// ball tracking
|
||||
bool is_tracked;
|
||||
is_tracked = tracker.processTracking();
|
||||
|
||||
if (start_following == true)
|
||||
{
|
||||
tracker.startTracking();
|
||||
follower.startFollowing();
|
||||
start_following = false;
|
||||
|
||||
wait_count = 1 * SPIN_RATE; // wait 1 sec
|
||||
}
|
||||
|
||||
if (stop_following == true)
|
||||
{
|
||||
follower.stopFollowing();
|
||||
stop_following = false;
|
||||
|
||||
wait_count = 0;
|
||||
}
|
||||
|
||||
if (wait_count <= 0)
|
||||
{
|
||||
// ball following
|
||||
if (on_following_ball == true)
|
||||
{
|
||||
if (is_tracked)
|
||||
follower.processFollowing(tracker.getPanOfBall(), tracker.getTiltOfBall(), tracker.getBallSize());
|
||||
else
|
||||
follower.waitFollowing();
|
||||
}
|
||||
|
||||
// check fallen states
|
||||
switch (stand_state)
|
||||
{
|
||||
case Stand:
|
||||
{
|
||||
// check restart soccer
|
||||
if (restart_soccer == true)
|
||||
{
|
||||
restart_soccer = false;
|
||||
startSoccerMode();
|
||||
break;
|
||||
}
|
||||
|
||||
// check states for kick
|
||||
int ball_position = follower.getBallPosition();
|
||||
if (ball_position != robotis_op::BallFollower::NotFound)
|
||||
{
|
||||
follower.stopFollowing();
|
||||
handleKick(ball_position);
|
||||
}
|
||||
break;
|
||||
}
|
||||
|
||||
// fallen state : Fallen_Forward, Fallen_Behind
|
||||
default:
|
||||
{
|
||||
follower.stopFollowing();
|
||||
handleFallen(stand_state);
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
wait_count -= 1;
|
||||
}
|
||||
|
||||
//execute pending callbacks
|
||||
ros::spinOnce();
|
||||
|
||||
//relax to fit output rate
|
||||
loop_rate.sleep();
|
||||
}
|
||||
|
||||
//exit program
|
||||
return 0;
|
||||
}
|
||||
|
||||
void callbackThread()
|
||||
{
|
||||
ros::NodeHandle nh(ros::this_node::getName());
|
||||
|
||||
// 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);
|
||||
buttuon_sub = nh.subscribe("/robotis/open_cr/button", 1, buttonHandlerCallback);
|
||||
demo_command_sub = nh.subscribe("/ball_tracker/command", 1, demoCommandCallback);
|
||||
imu_data_sub = nh.subscribe("/robotis/open_cr/imu", 1, imuDataCallback);
|
||||
|
||||
while (nh.ok())
|
||||
{
|
||||
ros::spinOnce();
|
||||
|
||||
usleep(1000);
|
||||
}
|
||||
}
|
||||
|
||||
void setBodyModuleToDemo(const std::string &body_module)
|
||||
{
|
||||
robotis_controller_msgs::JointCtrlModule control_msg;
|
||||
|
||||
//std::string body_module = "action_module";
|
||||
std::string head_module = "head_control_module";
|
||||
|
||||
// todo : remove hard coding
|
||||
for (int ix = 1; ix <= 20; ix++)
|
||||
{
|
||||
std::string joint_name;
|
||||
|
||||
if (getJointNameFromID(ix, joint_name) == false)
|
||||
continue;
|
||||
|
||||
control_msg.joint_name.push_back(joint_name);
|
||||
if (ix <= 18)
|
||||
control_msg.module_name.push_back(body_module);
|
||||
else
|
||||
control_msg.module_name.push_back(head_module);
|
||||
|
||||
}
|
||||
|
||||
// no control
|
||||
if (control_msg.joint_name.size() == 0)
|
||||
return;
|
||||
|
||||
module_control_pub.publish(control_msg);
|
||||
std::cout << "enable module of body : " << body_module << std::endl;
|
||||
}
|
||||
|
||||
void setModuleToDemo(const std::string &module_name)
|
||||
{
|
||||
robotis_controller_msgs::JointCtrlModule control_msg;
|
||||
|
||||
// todo : remove hard coding
|
||||
for (int ix = 1; ix <= 20; ix++)
|
||||
{
|
||||
std::string joint_name;
|
||||
|
||||
if (getJointNameFromID(ix, joint_name) == false)
|
||||
continue;
|
||||
|
||||
control_msg.joint_name.push_back(joint_name);
|
||||
control_msg.module_name.push_back(module_name);
|
||||
}
|
||||
|
||||
// no control
|
||||
if (control_msg.joint_name.size() == 0)
|
||||
return;
|
||||
|
||||
module_control_pub.publish(control_msg);
|
||||
std::cout << "enable module : " << module_name << std::endl;
|
||||
}
|
||||
|
||||
void parseJointNameFromYaml(const std::string &path)
|
||||
{
|
||||
YAML::Node doc;
|
||||
try
|
||||
{
|
||||
// load yaml
|
||||
doc = YAML::LoadFile(path.c_str());
|
||||
} catch (const std::exception& e)
|
||||
{
|
||||
ROS_ERROR("Fail to load id_joint table yaml.");
|
||||
return;
|
||||
}
|
||||
|
||||
// parse id_joint table
|
||||
YAML::Node _id_sub_node = doc["id_joint"];
|
||||
for (YAML::iterator _it = _id_sub_node.begin(); _it != _id_sub_node.end(); ++_it)
|
||||
{
|
||||
int _id;
|
||||
std::string _joint_name;
|
||||
|
||||
_id = _it->first.as<int>();
|
||||
_joint_name = _it->second.as<std::string>();
|
||||
|
||||
id_joint_table[_id] = _joint_name;
|
||||
joint_id_table[_joint_name] = _id;
|
||||
}
|
||||
}
|
||||
|
||||
// joint id -> joint name
|
||||
bool getJointNameFromID(const int &id, std::string &joint_name)
|
||||
{
|
||||
std::map<int, std::string>::iterator _iter;
|
||||
|
||||
_iter = id_joint_table.find(id);
|
||||
if (_iter == id_joint_table.end())
|
||||
return false;
|
||||
|
||||
joint_name = _iter->second;
|
||||
return true;
|
||||
}
|
||||
|
||||
// joint name -> joint id
|
||||
bool getIDFromJointName(const std::string &joint_name, int &id)
|
||||
{
|
||||
std::map<std::string, int>::iterator _iter;
|
||||
|
||||
_iter = joint_id_table.find(joint_name);
|
||||
if (_iter == joint_id_table.end())
|
||||
return false;
|
||||
|
||||
id = _iter->second;
|
||||
return true;
|
||||
}
|
||||
|
||||
void buttonHandlerCallback(const std_msgs::String::ConstPtr& msg)
|
||||
{
|
||||
if (msg->data == "start")
|
||||
{
|
||||
if (on_following_ball == true)
|
||||
stopSoccerMode();
|
||||
else
|
||||
startSoccerMode();
|
||||
}
|
||||
}
|
||||
|
||||
void demoCommandCallback(const std_msgs::String::ConstPtr &msg)
|
||||
{
|
||||
if (msg->data == "start")
|
||||
{
|
||||
if (on_following_ball == true)
|
||||
stopSoccerMode();
|
||||
else
|
||||
startSoccerMode();
|
||||
}
|
||||
else if (msg->data == "stop")
|
||||
{
|
||||
stopSoccerMode();
|
||||
}
|
||||
}
|
||||
|
||||
// check fallen states
|
||||
void imuDataCallback(const sensor_msgs::Imu::ConstPtr& msg)
|
||||
{
|
||||
if (stop_fallen_check == true)
|
||||
return;
|
||||
|
||||
Eigen::Quaterniond orientation(msg->orientation.w, msg->orientation.x, msg->orientation.y, msg->orientation.z);
|
||||
Eigen::MatrixXd rpy_orientation = robotis_framework::convertQuaternionToRPY(orientation);
|
||||
rpy_orientation *= (180 / M_PI);
|
||||
|
||||
double pitch = rpy_orientation.coeff(1, 0);
|
||||
|
||||
if (present_pitch == 0)
|
||||
present_pitch = pitch;
|
||||
else
|
||||
present_pitch = present_pitch * 0.5 + pitch * 0.5;
|
||||
|
||||
if (present_pitch > FALL_FORWARD_LIMIT)
|
||||
stand_state = Fallen_Forward;
|
||||
else if (present_pitch < FALL_BACK_LIMIT)
|
||||
stand_state = Fallen_Behind;
|
||||
else
|
||||
stand_state = Stand;
|
||||
}
|
||||
|
||||
void startSoccerMode()
|
||||
{
|
||||
setBodyModuleToDemo("walking_module");
|
||||
|
||||
usleep(10 * 1000);
|
||||
|
||||
ROS_INFO("Start Soccer Demo");
|
||||
on_following_ball = true;
|
||||
start_following = true;
|
||||
}
|
||||
|
||||
void stopSoccerMode()
|
||||
{
|
||||
ROS_INFO("Stop Soccer Demo");
|
||||
on_following_ball = false;
|
||||
stop_following = true;
|
||||
}
|
||||
|
||||
void handleKick(int ball_position)
|
||||
{
|
||||
usleep(500 * 1000);
|
||||
|
||||
// change to motion module
|
||||
setModuleToDemo("action_module");
|
||||
|
||||
usleep(1000 * 1000);
|
||||
|
||||
if (handleFallen(stand_state) == true)
|
||||
return;
|
||||
|
||||
// kick motion
|
||||
switch (ball_position)
|
||||
{
|
||||
case robotis_op::BallFollower::OnRight:
|
||||
std::cout << "Kick Motion [R]: " << ball_position << std::endl;
|
||||
playMotion(RightKick);
|
||||
break;
|
||||
|
||||
case robotis_op::BallFollower::OnLeft:
|
||||
std::cout << "Kick Motion [L]: " << ball_position << std::endl;
|
||||
playMotion(LeftKick);
|
||||
break;
|
||||
|
||||
default:
|
||||
break;
|
||||
}
|
||||
|
||||
on_following_ball = false;
|
||||
|
||||
usleep(2000 * 1000);
|
||||
|
||||
if (handleFallen(stand_state) == true)
|
||||
return;
|
||||
|
||||
// ceremony
|
||||
std::cout << "Go Ceremony!!!" << std::endl;
|
||||
playMotion(Ceremony);
|
||||
}
|
||||
|
||||
bool handleFallen(int fallen_status)
|
||||
{
|
||||
if (fallen_status == Stand)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
// change to motion module
|
||||
setModuleToDemo("action_module");
|
||||
|
||||
usleep(600 * 1000);
|
||||
|
||||
// getup motion
|
||||
switch (fallen_status)
|
||||
{
|
||||
case Fallen_Forward:
|
||||
std::cout << "Getup Motion [F]: " << std::endl;
|
||||
playMotion(GetUpFront);
|
||||
break;
|
||||
|
||||
case Fallen_Behind:
|
||||
std::cout << "Getup Motion [B]: " << std::endl;
|
||||
playMotion(GetUpBack);
|
||||
break;
|
||||
|
||||
default:
|
||||
break;
|
||||
}
|
||||
|
||||
usleep(500 * 1000);
|
||||
|
||||
if (on_following_ball == true)
|
||||
restart_soccer = true;
|
||||
|
||||
// reset state
|
||||
on_following_ball = false;
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
void playMotion(int motion_index)
|
||||
{
|
||||
std_msgs::Int32 motion_msg;
|
||||
motion_msg.data = motion_index;
|
||||
|
||||
motion_index_pub.publish(motion_msg);
|
||||
}
|
@ -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:
|
||||
|
Loading…
Reference in New Issue
Block a user