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 #9 from ROBOTIS-GIT/develop

Browse Source 
Develop
This commit is contained in:
Kayman 2018-03-22 16:41:45 +09:00 committed by GitHub
commit cb443c9f80
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
19 changed files with 459 additions and 175 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
ball_detector/include/ball_detector/ball_detector.h
View File

@ -83,13 +83,16 @@ class BallDetector
void printConfig();
void saveConfig();
void setInputImage(const cv::Mat & inIm);
void setInputImage(const cv::Mat & inIm, cv::Mat &in_filter_img);
void getOutputImage(cv::Mat & outIm);
void filterImage();
void filterImage(const cv::Mat &in_filter_img, cv::Mat &out_filter_img);
void makeFilterMask(const cv::Mat &source_img, cv::Mat &mask_img, int range);
void makeFilterMaskFromBall(const cv::Mat &source_img, cv::Mat &mask_img);
void inRangeHsv(const cv::Mat &input_img, const HsvFilter &filter_value, cv::Mat &output_img);
void mophology(const cv::Mat &intput_img, cv::Mat &output_img, int ellipse_size);
void houghDetection(const unsigned int imgEncoding);
void houghDetection2(const cv::Mat &input_hough);
void drawOutputImage();
//ros node handle

4
ball_detector/launch/ball_detector_params.yaml
View File

@ -7,8 +7,8 @@ hough_accum_th: 28
min_radius: 20
max_radius: 300
filter_h_min: 350
filter_h_max: 20
filter_s_min: 220
filter_h_max: 15
filter_s_min: 200
filter_s_max: 255
filter_v_min: 80
filter_v_max: 255

135
ball_detector/src/ball_detector.cpp
View File

@ -125,14 +125,25 @@ void BallDetector::process()
if (cv_img_ptr_sub_ != NULL)
{
//sets input image
setInputImage(cv_img_ptr_sub_->image);
cv::Mat img_hsv, img_filtered;
// test image filtering
filterImage();
// set input image
setInputImage(cv_img_ptr_sub_->image, img_hsv);
// image filtering
filterImage(img_hsv, img_filtered);
//detect circles
houghDetection(this->img_encoding_);
houghDetection2(img_filtered);
// // set input image
// setInputImage(cv_img_ptr_sub_->image);
// // image filtering
// filterImage();
// //detect circles
// houghDetection(this->img_encoding_);
}
}
@ -184,8 +195,8 @@ void BallDetector::publishCircles()
// left(-1), right(+1)
for (int idx = 0; idx < circles_.size(); idx++)
{
circles_msg_.circles[idx].x = circles_[idx][0] / in_image_.cols * 2 - 1; // x (-1 ~ 1)
circles_msg_.circles[idx].y = circles_[idx][1] / in_image_.rows * 2 - 1; // y (-1 ~ 1)
circles_msg_.circles[idx].x = circles_[idx][0] / out_image_.cols * 2 - 1; // x (-1 ~ 1)
circles_msg_.circles[idx].y = circles_[idx][1] / out_image_.rows * 2 - 1; // y (-1 ~ 1)
circles_msg_.circles[idx].z = circles_[idx][2]; // radius
}
@ -482,6 +493,14 @@ void BallDetector::setInputImage(const cv::Mat & inIm)
out_image_ = in_image_.clone();
}
void BallDetector::setInputImage(const cv::Mat & inIm, cv::Mat &in_filter_img)
{
cv::cvtColor(inIm, in_filter_img, cv::COLOR_RGB2HSV);
if (params_config_.debug == false)
out_image_ = inIm.clone();
}
void BallDetector::getOutputImage(cv::Mat & outIm)
{
this->drawOutputImage();
@ -522,6 +541,42 @@ void BallDetector::filterImage()
cv::cvtColor(img_filtered, in_image_, cv::COLOR_GRAY2RGB);
}
void BallDetector::filterImage(const cv::Mat &in_filter_img, cv::Mat &out_filter_img)
{
if (!in_filter_img.data)
return;
inRangeHsv(in_filter_img, params_config_.filter_threshold, out_filter_img);
// mophology : open and close
mophology(out_filter_img, out_filter_img, params_config_.ellipse_size);
if (params_config_.use_second_filter == true)
{
// mask
cv::Mat img_mask;
// check hsv range
cv::Mat img_filtered2;
inRangeHsv(in_filter_img, params_config_.filter2_threshold, img_filtered2);
makeFilterMaskFromBall(out_filter_img, img_mask);
cv::bitwise_and(img_filtered2, img_mask, img_filtered2);
// or
cv::bitwise_or(out_filter_img, img_filtered2, out_filter_img);
}
mophology(out_filter_img, out_filter_img, params_config_.ellipse_size);
// cv::cvtColor(img_filtered, in_image_, cv::COLOR_GRAY2RGB);
//draws results to output Image
if (params_config_.debug == true)
cv::cvtColor(out_filter_img, out_image_, cv::COLOR_GRAY2RGB);
// out_image_ = in_image_.clone();
}
void BallDetector::makeFilterMask(const cv::Mat &source_img, cv::Mat &mask_img, int range)
{
// source_img.
@ -707,6 +762,68 @@ void BallDetector::houghDetection(const unsigned int imgEncoding)
}
}
void BallDetector::houghDetection2(const cv::Mat &input_hough)
{
// cv::Mat gray_image;
std::vector<cv::Vec3f> circles_current;
std::vector<cv::Vec3f> prev_circles = circles_;
//clear previous circles
circles_.clear();
// If input image is RGB, convert it to gray
// if (imgEncoding == IMG_RGB8)
// cv::cvtColor(input_hough, gray_image, CV_RGB2GRAY);
//Reduce the noise so we avoid false circle detection
cv::GaussianBlur(input_hough, input_hough,
cv::Size(params_config_.gaussian_blur_size, params_config_.gaussian_blur_size),
params_config_.gaussian_blur_sigma);
double hough_accum_th = params_config_.hough_accum_th;
//Apply the Hough Transform to find the circles
cv::HoughCircles(input_hough, 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);
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++)
{
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++)
{
cv::Point2d prev_center = cv::Point(prev_circles[prev_ix][0], prev_circles[prev_ix][1]);
double prev_radius = prev_circles[prev_ix][2];
cv::Point2d diff = center - prev_center;
double radius_th = std::max(radius, prev_radius) * 0.75;
if (sqrt(diff.dot(diff)) < radius_th)
{
if (abs(radius - prev_radius) < radius_th)
{
circles_current[ix] = circles_current[ix] * alpha + prev_circles[prev_ix] * (1 - alpha);
}
break;
}
}
circles_.push_back(circles_current[ix]);
}
}
void BallDetector::drawOutputImage()
{
cv::Point center_position;
@ -714,8 +831,8 @@ void BallDetector::drawOutputImage()
size_t ii;
//draws results to output Image
if (params_config_.debug == true)
out_image_ = in_image_.clone();
// if (params_config_.debug == true)
// out_image_ = in_image_.clone();
for (ii = 0; ii < circles_.size(); ii++)
{

1
ball_detector/src/ball_detector_node.cpp
View File

@ -39,6 +39,7 @@ int main(int argc, char **argv)
detector.process();
detector.publishImage();
detector.publishCircles();
}
//execute pending callbacks

4
op3_demo/include/op3_demo/action_demo.h
View File

@ -29,6 +29,7 @@
#include "op3_demo/op_demo.h"
#include "robotis_controller_msgs/JointCtrlModule.h"
#include "robotis_controller_msgs/SetModule.h"
#include "op3_action_module_msgs/IsRunning.h"
namespace robotis_op
@ -86,7 +87,7 @@ class ActionDemo : public OPDemo
bool isActionRunning();
void setModuleToDemo(const std::string &module_name);
void callServiceSettingModule(const std::string &module_name);
void actionSetNameCallback(const std_msgs::String::ConstPtr& msg);
void buttonHandlerCallback(const std_msgs::String::ConstPtr& msg);
void demoCommandCallback(const std_msgs::String::ConstPtr &msg);
@ -99,6 +100,7 @@ class ActionDemo : public OPDemo
ros::Subscriber demo_command_sub_;
ros::ServiceClient is_running_client_;
ros::ServiceClient set_joint_module_client_;
std::map<int, std::string> action_sound_table_;
std::vector<int> play_list_;

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

@ -26,6 +26,7 @@
#include <ros/package.h>
#include <std_msgs/String.h>
#include <std_msgs/Int32.h>
//#include <std_msgs/Float64MultiArray.h>
#include <sensor_msgs/JointState.h>
#include "robotis_controller_msgs/JointCtrlModule.h"
@ -39,7 +40,7 @@ namespace robotis_op
// head tracking for looking the ball
class BallTracker
{
public:
public:
enum TrackingStatus
{
NotFound = -1,
@ -72,7 +73,7 @@ class BallTracker
return current_ball_bottom_;
}
protected:
protected:
const double FOV_WIDTH;
const double FOV_HEIGHT;
const int NOT_FOUND_THRESHOLD;
@ -92,6 +93,8 @@ class BallTracker
ros::Publisher head_joint_pub_;
ros::Publisher head_scan_pub_;
// ros::Publisher error_pub_;
ros::Publisher motion_index_pub_;
ros::Subscriber ball_position_sub_;
@ -107,7 +110,9 @@ class BallTracker
bool on_tracking_;
double current_ball_pan_, current_ball_tilt_;
double current_ball_bottom_;
double x_error_sum_, y_error_sum_;
ros::Time prev_time_;
double p_gain_, d_gain_, i_gain_;
};
}

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

@ -24,6 +24,7 @@
#include <ros/ros.h>
#include <ros/package.h>
#include <std_msgs/Bool.h>
#include <std_msgs/String.h>
#include <std_msgs/Int32.h>
#include <sensor_msgs/JointState.h>

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

@ -30,6 +30,8 @@
#include "robotis_math/robotis_linear_algebra.h"
#include "op3_action_module_msgs/IsRunning.h"
#include "robotis_controller_msgs/SyncWriteItem.h"
#include "robotis_controller_msgs/JointCtrlModule.h"
#include "robotis_controller_msgs/SetJointModule.h"
namespace robotis_op
{
@ -70,6 +72,7 @@ class SoccerDemo : public OPDemo
void setBodyModuleToDemo(const std::string &body_module, bool with_head_control = true);
void setModuleToDemo(const std::string &module_name);
void callServiceSettingModule(const robotis_controller_msgs::JointCtrlModule &modules);
void parseJointNameFromYaml(const std::string &path);
bool getJointNameFromID(const int &id, std::string &joint_name);
bool getIDFromJointName(const std::string &joint_name, int &id);
@ -101,6 +104,7 @@ class SoccerDemo : public OPDemo
ros::Subscriber imu_data_sub_;
ros::ServiceClient is_running_client_;
ros::ServiceClient set_joint_module_client_;
std::map<int, std::string> id_joint_table_;
std::map<std::string, int> joint_id_table_;

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

@ -30,6 +30,7 @@
#include "op3_demo/face_tracker.h"
#include "robotis_controller_msgs/SyncWriteItem.h"
#include "robotis_controller_msgs/SetModule.h"
namespace robotis_op
{
@ -59,19 +60,21 @@ class VisionDemo : public OPDemo
void demoCommandCallback(const std_msgs::String::ConstPtr &msg);
void setModuleToDemo(const std::string &module_name);
void callServiceSettingModule(const std::string &module_name);
FaceTracker face_tracker_;
ros::Publisher module_control_pub_;
ros::Publisher motion_index_pub_;
ros::Publisher rgb_led_pub_;
ros::Publisher rgb_led_pub_;
ros::Publisher face_tracking_command_pub_;
ros::Subscriber buttuon_sub_;
ros::Subscriber faceCoord_sub_;
ros::ServiceClient set_joint_module_client_;
geometry_msgs::Point face_position_;
bool is_tracking_;
int tracking_status_;
};

2
op3_demo/launch/demo.launch
View File

@ -23,6 +23,8 @@
<!-- robotis op3 demo -->
<node pkg="op3_demo" type="op_demo_node" name="op3_demo" output="screen">
<param name="grass_demo" type="bool" value="False" />
<param name="p_gain" value="0.45" />
<param name="d_gain" value="0.045" />
</node>
</launch>

1
op3_demo/launch/face_detection_op3.launch
View File

@ -17,5 +17,6 @@
<param name="displayed_Image" type="int" value="0" />
<!-- <param name="publish" type="int" value="2" /> -->
<param name="publish" type="int" value="3" />
<param name="start_condition" type="bool" value="false" />
</node>
</launch>

9
op3_demo/launch/self_test.launch
View File

@ -17,7 +17,14 @@
<!-- sound player -->
<node pkg="ros_madplay_player" type="ros_madplay_player" name="ros_madplay_player" output="screen"/>
<!-- web setting -->
<include file="$(find op3_web_setting_tool)/launch/web_setting_server.launch" />
<!-- robotis op3 self test demo -->
<node pkg="op3_demo" type="self_test_node" name="op3_self_test" output="screen"/>
<node pkg="op3_demo" type="self_test_node" name="op3_self_test" output="screen">
<param name="grass_demo" type="bool" value="False" />
<param name="p_gain" value="0.45" />
<param name="d_gain" value="0.045" />
</node>
</launch>

24
op3_demo/src/action/action_demo.cpp
View File

@ -53,8 +53,6 @@ void ActionDemo::setDemoEnable()
{
setModuleToDemo("action_module");
usleep(100 * 1000);
enable_ = true;
ROS_INFO_COND(DEBUG_PRINT, "Start ActionScript Demo");
@ -69,7 +67,7 @@ void ActionDemo::setDemoDisable()
stopProcess();
enable_ = false;
ROS_WARN("Set Action demo disable");
play_list_.resize(0);
}
@ -181,6 +179,7 @@ void ActionDemo::callbackThread()
buttuon_sub_ = nh.subscribe("/robotis/open_cr/button", 1, &ActionDemo::buttonHandlerCallback, this);
is_running_client_ = nh.serviceClient<op3_action_module_msgs::IsRunning>("/robotis/action/is_running");
set_joint_module_client_ = nh.serviceClient<robotis_controller_msgs::SetModule>("/robotis/set_present_ctrl_modules");
while (nh.ok())
{
@ -359,11 +358,22 @@ void ActionDemo::buttonHandlerCallback(const std_msgs::String::ConstPtr& msg)
void ActionDemo::setModuleToDemo(const std::string &module_name)
{
std_msgs::String control_msg;
control_msg.data = "action_module";
callServiceSettingModule(module_name);
ROS_INFO_STREAM("enable module : " << module_name);
}
module_control_pub_.publish(control_msg);
std::cout << "enable module : " << module_name << std::endl;
void ActionDemo::callServiceSettingModule(const std::string &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;
}
return ;
}
void ActionDemo::demoCommandCallback(const std_msgs::String::ConstPtr &msg)

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

@ -23,7 +23,7 @@ namespace robotis_op
BallFollower::BallFollower()
: nh_(ros::this_node::getName()),
FOV_WIDTH(26.4 * M_PI / 180),
FOV_WIDTH(35.2 * M_PI / 180),
FOV_HEIGHT(21.6 * M_PI / 180),
count_not_found_(0),
count_to_kick_(0),
@ -216,7 +216,7 @@ bool BallFollower::processFollowing(double x_angle, double y_angle, double ball_
"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("In range [%d]", count_to_kick_);
ROS_INFO("In range [%d | %d]", count_to_kick_, accum_ball_position_);
if (count_to_kick_ > 20)
{

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

@ -22,22 +22,32 @@ namespace robotis_op
{
BallTracker::BallTracker()
: nh_(ros::this_node::getName()),
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)
: nh_(ros::this_node::getName()),
FOV_WIDTH(35.2 * 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),
x_error_sum_(0),
y_error_sum_(0),
current_ball_bottom_(0),
tracking_status_(NotFound),
DEBUG_PRINT(false)
{
ros::NodeHandle param_nh("~");
p_gain_ = param_nh.param("p_gain", 0.4);
i_gain_ = param_nh.param("i_gain", 0.0);
d_gain_ = param_nh.param("d_gain", 0.0);
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_scan_pub_ = nh_.advertise<std_msgs::String>("/robotis/head_control/scan_command", 0);
// error_pub_ = nh_.advertise<std_msgs::Float64MultiArray>("/ball_tracker/errors", 0);
ball_position_sub_ = nh_.subscribe("/ball_detector_node/circle_set", 1, &BallTracker::ballPositionCallback, this);
ball_tracking_command_sub_ = nh_.subscribe("/ball_tracker/command", 1, &BallTracker::ballTrackerCommandCallback,
@ -90,9 +100,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);
current_ball_pan_ = 0;
current_ball_tilt_ = 0;
x_error_sum_ = 0;
y_error_sum_ = 0;
}
void BallTracker::setUsingHeadScan(bool use_scan)
@ -147,24 +158,26 @@ int BallTracker::processTracking()
switch (tracking_status)
{
case NotFound:
tracking_status_ = tracking_status;
return tracking_status;
case NotFound:
tracking_status_ = tracking_status;
current_ball_pan_ = 0;
current_ball_tilt_ = 0;
x_error_sum_ = 0;
y_error_sum_ = 0;
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 Waiting:
tracking_status_ = tracking_status;
return tracking_status;
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;
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;
default:
break;
}
ROS_INFO_STREAM_COND(DEBUG_PRINT, "--------------------------------------------------------------");
@ -176,21 +189,34 @@ int BallTracker::processTracking()
double delta_time = dur.nsec * 0.000000001 + dur.sec;
prev_time_ = curr_time;
// 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;
x_error_sum_ += x_error;
y_error_sum_ += y_error;
double x_error_target = x_error * p_gain_ + x_error_diff * d_gain_ + x_error_sum_ * i_gain_;
double y_error_target = y_error * p_gain_ + y_error_diff * d_gain_ + y_error_sum_ * i_gain_;
ROS_INFO_STREAM_COND(DEBUG_PRINT, "--------------------------------------------------------------");
// std_msgs::Float64MultiArray x_error_msg;
// x_error_msg.data.push_back(x_error);
// x_error_msg.data.push_back(x_error_diff);
// x_error_msg.data.push_back(x_error_sum_);
// x_error_msg.data.push_back(x_error * p_gain_);
// x_error_msg.data.push_back(x_error_diff * d_gain_);
// x_error_msg.data.push_back(x_error_sum_ * i_gain_);
// x_error_msg.data.push_back(x_error_target);
// error_pub_.publish(x_error_msg);
ROS_INFO_STREAM_COND(DEBUG_PRINT, "------------------------ " << tracking_status << " --------------------------------------");
ROS_INFO_STREAM_COND(DEBUG_PRINT, "error : " << (x_error * 180 / M_PI) << " | " << (y_error * 180 / M_PI));
ROS_INFO_STREAM_COND(
DEBUG_PRINT,
"error_diff : " << (x_error_diff * 180 / M_PI) << " | " << (y_error_diff * 180 / M_PI) << " | " << delta_time);
DEBUG_PRINT,
"error_diff : " << (x_error_diff * 180 / M_PI) << " | " << (y_error_diff * 180 / M_PI) << " | " << delta_time);
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);
DEBUG_PRINT,
"error_sum : " << (x_error_sum_ * 180 / M_PI) << " | " << (y_error_sum_ * 180 / M_PI));
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_ << " | time : " << delta_time);
// move head joint
publishHeadJoint(x_error_target, y_error_target);
@ -201,7 +227,6 @@ int BallTracker::processTracking()
current_ball_bottom_ = ball_size;
ball_position_.z = 0;
//count_not_found_ = 0;
tracking_status_ = tracking_status;
return tracking_status;

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

@ -22,20 +22,20 @@ namespace robotis_op
{
SoccerDemo::SoccerDemo()
: FALL_FORWARD_LIMIT(60),
FALL_BACK_LIMIT(-60),
SPIN_RATE(30),
DEBUG_PRINT(false),
wait_count_(0),
on_following_ball_(false),
restart_soccer_(false),
start_following_(false),
stop_following_(false),
stop_fallen_check_(false),
robot_status_(Waited),
stand_state_(Stand),
tracking_status_(BallTracker::Waiting),
present_pitch_(0)
: FALL_FORWARD_LIMIT(60),
FALL_BACK_LIMIT(-60),
SPIN_RATE(30),
DEBUG_PRINT(false),
wait_count_(0),
on_following_ball_(false),
restart_soccer_(false),
start_following_(false),
stop_following_(false),
stop_fallen_check_(false),
robot_status_(Waited),
stand_state_(Stand),
tracking_status_(BallTracker::Waiting),
present_pitch_(0)
{
//init ros
enable_ = false;
@ -66,8 +66,6 @@ void SoccerDemo::setDemoEnable()
void SoccerDemo::setDemoDisable()
{
setModuleToDemo("base_module");
// handle disable procedure
ball_tracker_.stopTracking();
ball_follower_.stopFollowing();
@ -85,8 +83,10 @@ void SoccerDemo::setDemoDisable()
void SoccerDemo::process()
{
if(enable_ == false)
return;
// ball tracking
bool is_tracked;
int tracking_status;
tracking_status = ball_tracker_.processTracking();
@ -101,9 +101,6 @@ void SoccerDemo::process()
wait_count_ = 1 * SPIN_RATE;
}
//for debug
//return;
// check to stop
if (stop_following_ == true)
{
@ -121,20 +118,20 @@ void SoccerDemo::process()
{
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::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;
case BallTracker::NotFound:
ball_follower_.waitFollowing();
if(tracking_status_ != tracking_status)
setRGBLED(0, 0, 0);
break;
default:
break;
default:
break;
}
if(tracking_status != tracking_status_)
@ -144,32 +141,32 @@ void SoccerDemo::process()
// check fallen states
switch (stand_state_)
{
case Stand:
case Stand:
{
// check restart soccer
if (restart_soccer_ == true)
{
// check restart soccer
if (restart_soccer_ == true)
{
restart_soccer_ = false;
startSoccerMode();
break;
}
// check states for kick
int ball_position = ball_follower_.getBallPosition();
if (ball_position != robotis_op::BallFollower::NotFound)
{
ball_follower_.stopFollowing();
handleKick(ball_position);
}
restart_soccer_ = false;
startSoccerMode();
break;
}
// fallen state : Fallen_Forward, Fallen_Behind
default:
// check states for kick
int ball_position = ball_follower_.getBallPosition();
if (ball_position != robotis_op::BallFollower::NotFound)
{
ball_follower_.stopFollowing();
handleFallen(stand_state_);
break;
handleKick(ball_position);
}
break;
}
// fallen state : Fallen_Forward, Fallen_Behind
default:
{
ball_follower_.stopFollowing();
handleFallen(stand_state_);
break;
}
}
}
else
@ -212,6 +209,7 @@ void SoccerDemo::callbackThread()
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");
set_joint_module_client_ = nh.serviceClient<robotis_controller_msgs::SetJointModule>("/robotis/set_present_joint_ctrl_modules");
while (nh.ok())
{
@ -252,12 +250,15 @@ void SoccerDemo::setBodyModuleToDemo(const std::string &body_module, bool with_h
if (control_msg.joint_name.size() == 0)
return;
module_control_pub_.publish(control_msg);
callServiceSettingModule(control_msg);
std::cout << "enable module of body : " << body_module << std::endl;
}
void SoccerDemo::setModuleToDemo(const std::string &module_name)
{
if(enable_ == false)
return;
robotis_controller_msgs::JointCtrlModule control_msg;
std::map<int, std::string>::iterator joint_iter;
@ -271,10 +272,25 @@ void SoccerDemo::setModuleToDemo(const std::string &module_name)
if (control_msg.joint_name.size() == 0)
return;
module_control_pub_.publish(control_msg);
callServiceSettingModule(control_msg);
std::cout << "enable module : " << module_name << std::endl;
}
void SoccerDemo::callServiceSettingModule(const robotis_controller_msgs::JointCtrlModule &modules)
{
robotis_controller_msgs::SetJointModule set_joint_srv;
set_joint_srv.request.joint_name = modules.joint_name;
set_joint_srv.request.module_name = modules.module_name;
if (set_joint_module_client_.call(set_joint_srv) == false)
{
ROS_ERROR("Failed to set module");
return;
}
return ;
}
void SoccerDemo::parseJointNameFromYaml(const std::string &path)
{
YAML::Node doc;
@ -414,16 +430,10 @@ void SoccerDemo::startSoccerMode()
{
setModuleToDemo("action_module");
usleep(100 * 1000);
playMotion(WalkingReady);
usleep(1500 * 1000);
setBodyModuleToDemo("walking_module");
usleep(20 * 1000);
ROS_INFO("Start Soccer Demo");
on_following_ball_ = true;
start_following_ = true;
@ -445,24 +455,24 @@ void SoccerDemo::handleKick(int ball_position)
usleep(1500 * 1000);
if (handleFallen(stand_state_) == true)
if (handleFallen(stand_state_) == true || enable_ == false)
return;
// kick motion
switch (ball_position)
{
case robotis_op::BallFollower::OnRight:
std::cout << "Kick Motion [R]: " << ball_position << std::endl;
playMotion(is_grass_ ? RightKick + ForGrass : RightKick);
break;
case robotis_op::BallFollower::OnRight:
std::cout << "Kick Motion [R]: " << ball_position << std::endl;
playMotion(is_grass_ ? RightKick + ForGrass : RightKick);
break;
case robotis_op::BallFollower::OnLeft:
std::cout << "Kick Motion [L]: " << ball_position << std::endl;
playMotion(is_grass_ ? LeftKick + ForGrass : LeftKick);
break;
case robotis_op::BallFollower::OnLeft:
std::cout << "Kick Motion [L]: " << ball_position << std::endl;
playMotion(is_grass_ ? LeftKick + ForGrass : LeftKick);
break;
default:
break;
default:
break;
}
on_following_ball_ = false;
@ -487,23 +497,21 @@ bool SoccerDemo::handleFallen(int fallen_status)
// 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(is_grass_ ? GetUpFront + ForGrass : GetUpFront);
break;
case Fallen_Forward:
std::cout << "Getup Motion [F]: " << std::endl;
playMotion(is_grass_ ? GetUpFront + ForGrass : GetUpFront);
break;
case Fallen_Behind:
std::cout << "Getup Motion [B]: " << std::endl;
playMotion(is_grass_ ? GetUpBack + ForGrass : GetUpBack);
break;
case Fallen_Behind:
std::cout << "Getup Motion [B]: " << std::endl;
playMotion(is_grass_ ? GetUpBack + ForGrass : GetUpBack);
break;
default:
break;
default:
break;
}
while(isActionRunning() == true)

97
op3_demo/src/test_node.cpp
View File

@ -44,12 +44,17 @@ void goInitPose();
void playSound(const std::string &path);
void setLED(int led);
bool checkManagerRunning(std::string& manager_name);
void dxlTorqueChecker();
void demoModeCommandCallback(const std_msgs::String::ConstPtr &msg);
const int SPIN_RATE = 30;
const bool DEBUG_PRINT = true;
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;
@ -75,7 +80,9 @@ 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);
ros::Subscriber mode_command_sub = nh.subscribe("/robotis/mode_command", 1, demoModeCommandCallback);
default_mp3_path = ros::package::getPath("op3_demo") + "/Data/mp3/";
@ -93,7 +100,7 @@ int main(int argc, char **argv)
if (checkManagerRunning(manager_name) == true)
{
break;
ROS_INFO("Succeed to connect");
ROS_INFO_COND(DEBUG_PRINT, "Succeed to connect");
}
ROS_WARN("Waiting for op3 manager");
}
@ -120,7 +127,7 @@ int main(int argc, char **argv)
goInitPose();
ROS_INFO("[Go to Demo READY!]");
ROS_INFO_COND(DEBUG_PRINT, "[Go to Demo READY!]");
break;
}
@ -132,7 +139,7 @@ int main(int argc, char **argv)
current_demo = soccer_demo;
current_demo->setDemoEnable();
ROS_INFO("[Start] Soccer Demo");
ROS_INFO_COND(DEBUG_PRINT, "[Start] Soccer Demo");
break;
}
@ -143,7 +150,7 @@ int main(int argc, char **argv)
current_demo = vision_demo;
current_demo->setDemoEnable();
ROS_INFO("[Start] Vision Demo");
ROS_INFO_COND(DEBUG_PRINT, "[Start] Vision Demo");
break;
}
case ActionDemo:
@ -153,7 +160,7 @@ int main(int argc, char **argv)
current_demo = action_demo;
current_demo->setDemoEnable();
ROS_INFO("[Start] Action Demo");
ROS_INFO_COND(DEBUG_PRINT, "[Start] Action Demo");
break;
}
case ButtonTest:
@ -163,7 +170,7 @@ int main(int argc, char **argv)
current_demo = button_test;
current_demo->setDemoEnable();
ROS_INFO("[Start] Button Test");
ROS_INFO_COND(DEBUG_PRINT, "[Start] Button Test");
break;
}
case MicTest:
@ -173,7 +180,7 @@ int main(int argc, char **argv)
current_demo = mic_test;
current_demo->setDemoEnable();
ROS_INFO("[Start] Mic Test");
ROS_INFO_COND(DEBUG_PRINT, "[Start] Mic Test");
break;
}
@ -193,6 +200,10 @@ int main(int argc, char **argv)
//relax to fit output rate
loop_rate.sleep();
// for debug
if (checkManagerRunning(manager_name) == false)
return 0;
}
//exit program
@ -201,6 +212,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)
{
@ -232,22 +246,27 @@ 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;
case ButtonTest:
dxlTorqueChecker();
playSound(default_mp3_path + "test/Start button test mode.mp3");
break;
case MicTest:
dxlTorqueChecker();
playSound(default_mp3_path + "test/Start mic test mode.mp3");
break;
@ -255,7 +274,7 @@ void buttonHandlerCallback(const std_msgs::String::ConstPtr& msg)
break;
}
ROS_INFO("= Start Demo Mode : %d", desired_status);
ROS_INFO_COND(DEBUG_PRINT, "= Start Demo Mode : %d", desired_status);
}
else if (msg->data == "mode")
{
@ -295,7 +314,7 @@ void buttonHandlerCallback(const std_msgs::String::ConstPtr& msg)
break;
}
ROS_INFO("= Demo Mode : %d", desired_status);
ROS_INFO_COND(DEBUG_PRINT, "= Demo Mode : %d", desired_status);
}
}
}
@ -340,3 +359,63 @@ 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);
}
void demoModeCommandCallback(const std_msgs::String::ConstPtr &msg)
{
// In demo mode
if (current_status != Ready)
{
if (msg->data == "ready")
{
// go to mode selection status
desired_status = Ready;
apply_desired = true;
playSound(default_mp3_path + "Demonstration ready mode.mp3");
setLED(0x01 | 0x02 | 0x04);
}
}
// In ready mode
else
{
if(msg->data == "soccer")
{
desired_status = SoccerDemo;
apply_desired = true;
// play sound
dxlTorqueChecker();
playSound(default_mp3_path + "Start soccer demonstration.mp3");
ROS_INFO_COND(DEBUG_PRINT, "= Start Demo Mode : %d", desired_status);
}
else if(msg->data == "vision")
{
desired_status = VisionDemo;
apply_desired = true;
// play sound
dxlTorqueChecker();
playSound(default_mp3_path + "Start vision processing demonstration.mp3");
ROS_INFO_COND(DEBUG_PRINT, "= Start Demo Mode : %d", desired_status);
}
else if(msg->data == "action")
{
desired_status = ActionDemo;
apply_desired = true;
// play sound
dxlTorqueChecker();
playSound(default_mp3_path + "Start motion demonstration.mp3");
ROS_INFO_COND(DEBUG_PRINT, "= Start Demo Mode : %d", desired_status);
}
}
}

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

@ -23,7 +23,7 @@ namespace robotis_op
FaceTracker::FaceTracker()
: nh_(ros::this_node::getName()),
FOV_WIDTH(26.4 * M_PI / 180),
FOV_WIDTH(35.2 * M_PI / 180),
FOV_HEIGHT(21.6 * M_PI / 180),
NOT_FOUND_THRESHOLD(50),
use_head_scan_(false),
@ -34,8 +34,7 @@ FaceTracker::FaceTracker()
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);
face_tracking_command_sub_ = nh_.subscribe("/face_tracker/command", 1, &FaceTracker::faceTrackerCommandCallback,
this);
//face_tracking_command_sub_ = nh_.subscribe("/robotis/demo_command", 1, &FaceTracker::faceTrackerCommandCallback, this);
}
FaceTracker::~FaceTracker()
@ -73,12 +72,14 @@ void FaceTracker::faceTrackerCommandCallback(const std_msgs::String::ConstPtr &m
void FaceTracker::startTracking()
{
on_tracking_ = true;
ROS_INFO("Start Face tracking");
}
void FaceTracker::stopTracking()
{
on_tracking_ = false;
ROS_INFO("Stop Face tracking");
}

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

@ -23,7 +23,6 @@ namespace robotis_op
VisionDemo::VisionDemo()
: SPIN_RATE(30),
is_tracking_(false),
tracking_status_(FaceTracker::Waiting)
{
enable_ = false;
@ -44,17 +43,19 @@ void VisionDemo::setDemoEnable()
// change to motion module
setModuleToDemo("action_module");
usleep(100 * 1000);
playMotion(InitPose);
usleep(1500 * 1000);
setModuleToDemo("head_control_module");
usleep(20 * 1000);
enable_ = true;
// send command to start face_tracking
std_msgs::Bool command;
command.data = enable_;
face_tracking_command_pub_.publish(command);
face_tracker_.startTracking();
ROS_INFO("Start Vision Demo");
@ -65,9 +66,12 @@ void VisionDemo::setDemoDisable()
{
face_tracker_.stopTracking();
is_tracking_ = false;
tracking_status_ = FaceTracker::Waiting;
enable_ = false;
std_msgs::Bool command;
command.data = enable_;
face_tracking_command_pub_.publish(command);
}
void VisionDemo::process()
@ -93,9 +97,6 @@ void VisionDemo::process()
if(tracking_status != FaceTracker::Waiting)
tracking_status_ = tracking_status;
//is_tracking_ = is_tracked;
std::cout << "Tracking : " << tracking_status << std::endl;
}
void VisionDemo::processThread()
@ -122,10 +123,13 @@ void VisionDemo::callbackThread()
module_control_pub_ = nh.advertise<std_msgs::String>("/robotis/enable_ctrl_module", 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);
face_tracking_command_pub_ = nh.advertise<std_msgs::Bool>("/face_tracking/command", 0);
buttuon_sub_ = nh.subscribe("/robotis/open_cr/button", 1, &VisionDemo::buttonHandlerCallback, this);
faceCoord_sub_ = nh.subscribe("/faceCoord", 1, &VisionDemo::facePositionCallback, this);
set_joint_module_client_ = nh.serviceClient<robotis_controller_msgs::SetModule>("/robotis/set_present_ctrl_modules");
while (nh.ok())
{
ros::spinOnce();
@ -166,11 +170,22 @@ void VisionDemo::demoCommandCallback(const std_msgs::String::ConstPtr &msg)
void VisionDemo::setModuleToDemo(const std::string &module_name)
{
std_msgs::String control_msg;
control_msg.data = module_name;
callServiceSettingModule(module_name);
ROS_INFO_STREAM("enable module : " << module_name);
}
module_control_pub_.publish(control_msg);
std::cout << "enable module : " << module_name << std::endl;
void VisionDemo::callServiceSettingModule(const std::string &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;
}
return ;
}
void VisionDemo::facePositionCallback(const std_msgs::Int32MultiArray::ConstPtr &msg)