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

Browse Source 
Develop
This commit is contained in:
Kayman 2018-02-28 15:44:12 +09:00 committed by GitHub
commit b76013c817
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
17 changed files with 350 additions and 86 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
ball_detector/CMakeLists.txt
View File

@ -8,6 +8,7 @@ project(ball_detector)
# Packages
################################################################################
find_package(catkin REQUIRED COMPONENTS
roslib
cv_bridge
geometry_msgs
image_transport
@ -24,6 +25,13 @@ find_package(catkin REQUIRED COMPONENTS
add_message_files(
FILES
circleSetStamped.msg
BallDetectorParams.msg
)
add_service_files(
FILES
GetParameters.srv
SetParameters.srv
)
generate_messages(
@ -70,4 +78,4 @@ target_link_libraries(ball_detector_node
################################################################################
# Test
################################################################################
################################################################################

17
ball_detector/include/ball_detector/ball_detector.h
View File

@ -26,7 +26,9 @@
//ros dependencies
#include <ros/ros.h>
#include <ros/package.h>
#include <std_msgs/Bool.h>
#include <std_msgs/String.h>
#include <image_transport/image_transport.h>
#include <cv_bridge/cv_bridge.h>
#include <sensor_msgs/CameraInfo.h>
@ -36,6 +38,8 @@
#include "ball_detector/circleSetStamped.h"
#include "ball_detector/ball_detector_config.h"
#include "ball_detector/detectorParamsConfig.h"
#include "ball_detector/GetParameters.h"
#include "ball_detector/SetParameters.h"
namespace robotis_op
{
@ -70,6 +74,12 @@ class BallDetector
void dynParamCallback(ball_detector::detectorParamsConfig &config, uint32_t level);
void enableCallback(const std_msgs::Bool::ConstPtr &msg);
void paramCommandCallback(const std_msgs::String::ConstPtr &msg);
bool setParamCallback(ball_detector::SetParameters::Request &req, ball_detector::SetParameters::Response &res);
bool getParamCallback(ball_detector::GetParameters::Request &req, ball_detector::GetParameters::Response &res);
void resetParameter();
void publishParam();
void printConfig();
void saveConfig();
void setInputImage(const cv::Mat & inIm);
@ -112,6 +122,13 @@ class BallDetector
std::string param_path_;
bool has_path_;
// web setting
std::string default_setting_path_;
ros::Publisher param_pub_;
ros::Subscriber param_command_sub_;
ros::ServiceServer get_param_client_;
ros::ServiceServer set_param_client_;
//flag indicating a new image has been received
bool new_image_flag_;

35
ball_detector/launch/ball_detector_from_usb_cam.launch
View File

@ -1,37 +1,28 @@
<?xml version="1.0"?>
<!-- Launches an UVC camera, the ball detector and its visualization -->
<launch>
<launch>
<!-- UVC camera -->
<node pkg="usb_cam" type="usb_cam_node" name="usb_cam_node" output="screen">
<param name="video_device" type="string" value="/dev/video0" />
<param name="image_width" type="int" value="800" />
<param name="image_height" type="int" value="600" />
<!-- <param name="image_width" type="int" value="1600" />
<param name="image_height" type="int" value="896" /> -->
<param name="image_width" type="int" value="1280" />
<param name="image_height" type="int" value="720" />
<param name="framerate " type="int" value="30" />
<param name="camera_frame_id" type="string" value="cam_link" />
<param name="camera_name" type="string" value="camera" />
<param name="autofocus" type="bool" value="False" />
<param name="autoexposure" type="bool" value="False" />
<param name="auto_white_balance" type="bool" value="False" />
<param name="gain" value="255" />
<param name="brightness" value="64" />
<param name="exposure" value="80" />
</node>
<!-- <param name="gain" value="255" />
<param name="auto_exposure" type="bool" value="False" />
<param name="exposure_absolute" value="1000" />
<param name="brightness" value="127" />
<param name="auto_white_balance" type="bool" value="False" />
<param name="white_balance_temperature" value="2800" /> -->
<!-- <param name="auto_exposure" type="bool" value="False" /> -->
<!-- <param name="exposure_absolute" value="1000" /> -->
<!-- <param name="autofocus" type="bool" value="False" /> -->
<!-- <param name="autoexposure" type="bool" value="False" /> -->
<!-- <param name="auto_white_balance" type="bool" value="False" /> -->
<!-- <param name="gain" value="255" /> -->
<!-- <param name="brightness" value="64" /> -->
<!-- <param name="exposure" value="80" /> -->
<!-- <param name="auto_exposure" type="bool" value="False" /> -->
<!-- <param name="exposure_absolute" value="1000" /> -->
<!-- <param name="auto_white_balance" type="bool" value="False" /> -->
<!-- <param name="white_balance_temperature" value="2800" /> -->
<!-- <param name="camera_info_url" type="string" value="file://$(find ar_pose)/data/camera_1280720.yaml" /> -->
</node>
<!-- ball detector -->
<include file="$(find ball_detector)/launch/ball_detector.launch" />
</launch>

14
ball_detector/launch/ball_detector_params.yaml
View File

@ -8,16 +8,16 @@ min_radius: 20
max_radius: 300
filter_h_min: 350
filter_h_max: 20
filter_s_min: 230
filter_s_min: 220
filter_s_max: 255
filter_v_min: 30
filter_v_min: 80
filter_v_max: 255
use_second_filter: false
filter2_h_min: 40
filter2_h_max: 345
filter2_h_min: 30
filter2_h_max: 355
filter2_s_min: 0
filter2_s_max: 44
filter2_v_min: 90
filter2_s_max: 40
filter2_v_min: 200
filter2_v_max: 255
ellipse_size: 3
ellipse_size: 1
filter_debug: false

20
ball_detector/launch/ball_detector_params_default.yaml
View File

@ -1,18 +1,18 @@
gaussian_blur_size: 7
gaussian_blur_sigma: 2.52
canny_edge_th: 100.5
gaussian_blur_sigma: 2.0
canny_edge_th: 100
hough_accum_resolution: 1
min_circle_dist: 28.5
hough_accum_th: 26.6
min_radius: 25
max_radius: 150
min_circle_dist: 100
hough_accum_th: 28
min_radius: 20
max_radius: 300
filter_h_min: 350
filter_h_max: 20
filter_s_min: 90
filter_s_min: 220
filter_s_max: 255
filter_v_min: 86
filter_v_min: 80
filter_v_max: 255
use_second_filter: true
use_second_filter: false
filter2_h_min: 30
filter2_h_max: 355
filter2_s_min: 0
@ -20,4 +20,4 @@ filter2_s_max: 40
filter2_v_min: 200
filter2_v_max: 255
ellipse_size: 1
filter_debug: false
filter_debug: false

17
ball_detector/msg/BallDetectorParams.msg Normal file
View File

@ -0,0 +1,17 @@
# This represents the parameters of ball_detector
uint32 gaussian_blur_size # only odd number, 1 - 11
float32 gaussian_blur_sigma # 1 - 5
float32 canny_edge_th # 50 - 200
float32 hough_accum_resolution # 1 - 8
float32 hough_accum_th # 10 - 200
float32 min_circle_dist # 10 - 200
uint32 min_radius # 10 - 200
uint32 max_radius # 100 - 600
uint32 filter_h_min # 0 - 359
uint32 filter_h_max
uint32 filter_s_min # 0 - 255
uint32 filter_s_max
uint32 filter_v_min # 0 - 255
uint32 filter_v_max
uint32 ellipse_size # 1 - 9

4
ball_detector/package.xml
View File

@ -11,7 +11,8 @@
<author email="kmjung@robotis.com">kayman</author>
<maintainer email="pyo@robotis.com">Pyo</maintainer>
<buildtool_depend>catkin</buildtool_depend>
<buildtool_depend>catkin</buildtool_depend>
<build_depend>roslib</build_depend>
<build_depend>cv_bridge</build_depend>
<build_depend>geometry_msgs</build_depend>
<build_depend>image_transport</build_depend>
@ -20,6 +21,7 @@
<build_depend>std_msgs</build_depend>
<build_depend>dynamic_reconfigure</build_depend>
<build_depend>message_generation</build_depend>
<run_depend>roslib</run_depend>
<run_depend>cv_bridge</run_depend>
<run_depend>geometry_msgs</run_depend>
<run_depend>image_transport</run_depend>

177
ball_detector/src/ball_detector.cpp
View File

@ -25,12 +25,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_);
@ -87,6 +87,13 @@ BallDetector::BallDetector()
callback_fnc_ = boost::bind(&BallDetector::dynParamCallback, this, _1, _2);
param_server_.setCallback(callback_fnc_);
// web setting
param_pub_ = nh_.advertise<ball_detector::BallDetectorParams>("current_params", 1);
param_command_sub_ = nh_.subscribe("param_command", 1, &BallDetector::paramCommandCallback, this);
set_param_client_ = nh_.advertiseService("set_param", &BallDetector::setParamCallback, this);
get_param_client_ = nh_.advertiseService("get_param", &BallDetector::getParamCallback, this);
default_setting_path_ = ros::package::getPath(ROS_PACKAGE_NAME) + "/launch/ball_detector_params_default.yaml";
//sets config and prints it
params_config_ = detect_config;
init_param_ = true;
@ -140,15 +147,15 @@ void BallDetector::publishImage()
cv_img_pub_.header.frame_id = image_frame_id_;
switch (img_encoding_)
{
case IMG_RGB8:
cv_img_pub_.encoding = sensor_msgs::image_encodings::RGB8;
break;
case IMG_MONO:
cv_img_pub_.encoding = sensor_msgs::image_encodings::MONO8;
break;
default:
cv_img_pub_.encoding = sensor_msgs::image_encodings::MONO8;
break;
case IMG_RGB8:
cv_img_pub_.encoding = sensor_msgs::image_encodings::RGB8;
break;
case IMG_MONO:
cv_img_pub_.encoding = sensor_msgs::image_encodings::MONO8;
break;
default:
cv_img_pub_.encoding = sensor_msgs::image_encodings::MONO8;
break;
}
getOutputImage(cv_img_pub_.image);
image_pub_.publish(cv_img_pub_.toImageMsg());
@ -260,6 +267,146 @@ void BallDetector::cameraInfoCallback(const sensor_msgs::CameraInfo & msg)
camera_info_msg_ = msg;
}
void BallDetector::paramCommandCallback(const std_msgs::String::ConstPtr &msg)
{
if(msg->data == "debug")
{
params_config_.debug = true;
saveConfig();
}
else if(msg->data == "normal")
{
params_config_.debug = false;
saveConfig();
}
else if(msg->data == "reset")
{
// load default parameters and apply
resetParameter();
}
}
bool BallDetector::setParamCallback(ball_detector::SetParameters::Request &req, ball_detector::SetParameters::Response &res)
{
params_config_.gaussian_blur_size = req.params.gaussian_blur_size;
params_config_.gaussian_blur_sigma = req.params.gaussian_blur_sigma;
params_config_.canny_edge_th = req.params.canny_edge_th;
params_config_.hough_accum_resolution = req.params.hough_accum_resolution;
params_config_.min_circle_dist = req.params.min_circle_dist;
params_config_.hough_accum_th = req.params.hough_accum_th;
params_config_.min_radius = req.params.min_radius;
params_config_.max_radius = req.params.max_radius;
params_config_.filter_threshold.h_min = req.params.filter_h_min;
params_config_.filter_threshold.h_max = req.params.filter_h_max;
params_config_.filter_threshold.s_min = req.params.filter_s_min;
params_config_.filter_threshold.s_max = req.params.filter_s_max;
params_config_.filter_threshold.v_min = req.params.filter_v_min;
params_config_.filter_threshold.v_max = req.params.filter_v_max;
params_config_.ellipse_size = req.params.ellipse_size;
saveConfig();
res.returns = req.params;
return true;
}
bool BallDetector:: getParamCallback(ball_detector::GetParameters::Request &req, ball_detector::GetParameters::Response &res)
{
res.returns.gaussian_blur_size = params_config_.gaussian_blur_size;
res.returns.gaussian_blur_sigma = params_config_.gaussian_blur_sigma;
res.returns.canny_edge_th = params_config_.canny_edge_th;
res.returns.hough_accum_resolution = params_config_.hough_accum_resolution;
res.returns.min_circle_dist = params_config_.min_circle_dist;
res.returns.hough_accum_th = params_config_.hough_accum_th;
res.returns.min_radius = params_config_.min_radius;
res.returns.max_radius = params_config_.max_radius;
res.returns.filter_h_min = params_config_.filter_threshold.h_min;
res.returns.filter_h_max = params_config_.filter_threshold.h_max;
res.returns.filter_s_min = params_config_.filter_threshold.s_min;
res.returns.filter_s_max = params_config_.filter_threshold.s_max;
res.returns.filter_v_min = params_config_.filter_threshold.v_min;
res.returns.filter_v_max = params_config_.filter_threshold.v_max;
res.returns.ellipse_size = params_config_.ellipse_size;
return true;
}
void BallDetector::resetParameter()
{
YAML::Node doc;
try
{
// load yaml
doc = YAML::LoadFile(default_setting_path_.c_str());
// parse
params_config_.gaussian_blur_size = doc["gaussian_blur_size"].as<int>();
params_config_.gaussian_blur_sigma = doc["gaussian_blur_sigma"].as<double>();
params_config_.canny_edge_th = doc["canny_edge_th"].as<double>();
params_config_.hough_accum_resolution = doc["hough_accum_resolution"].as<double>();
params_config_.min_circle_dist = doc["min_circle_dist"].as<double>();
params_config_.hough_accum_th = doc["hough_accum_th"].as<double>();
params_config_.min_radius = doc["min_radius"].as<int>();
params_config_.max_radius = doc["max_radius"].as<int>();
params_config_.filter_threshold.h_min = doc["filter_h_min"].as<int>();
params_config_.filter_threshold.h_max = doc["filter_h_max"].as<int>();
params_config_.filter_threshold.s_min = doc["filter_s_min"].as<int>();
params_config_.filter_threshold.s_max = doc["filter_s_max"].as<int>();
params_config_.filter_threshold.v_min = doc["filter_v_min"].as<int>();
params_config_.filter_threshold.v_max = doc["filter_v_max"].as<int>();
params_config_.use_second_filter = doc["use_second_filter"].as<bool>();
params_config_.filter2_threshold.h_min = doc["filter2_h_min"].as<int>();
params_config_.filter2_threshold.h_max = doc["filter2_h_max"].as<int>();
params_config_.filter2_threshold.s_min = doc["filter2_s_min"].as<int>();
params_config_.filter2_threshold.s_max = doc["filter2_s_max"].as<int>();
params_config_.filter2_threshold.v_min = doc["filter2_v_min"].as<int>();
params_config_.filter2_threshold.v_max = doc["filter2_v_max"].as<int>();
params_config_.ellipse_size = doc["ellipse_size"].as<int>();
params_config_.debug = doc["filter_debug"].as<bool>();
// gaussian_blur has to be odd number.
if (params_config_.gaussian_blur_size % 2 == 0)
params_config_.gaussian_blur_size -= 1;
if (params_config_.gaussian_blur_size <= 0)
params_config_.gaussian_blur_size = 1;
printConfig();
saveConfig();
publishParam();
} catch (const std::exception& e)
{
ROS_ERROR_STREAM("Failed to Get default parameters : " << default_setting_path_);
return;
}
}
void BallDetector::publishParam()
{
ball_detector::BallDetectorParams params;
params.gaussian_blur_size = params_config_.gaussian_blur_size;
params.gaussian_blur_sigma = params_config_.gaussian_blur_sigma;
params.canny_edge_th = params_config_.canny_edge_th;
params.hough_accum_resolution = params_config_.hough_accum_resolution;
params.min_circle_dist = params_config_.min_circle_dist;
params.hough_accum_th = params_config_.hough_accum_th;
params.min_radius = params_config_.min_radius;
params.max_radius = params_config_.max_radius;
params.filter_h_min = params_config_.filter_threshold.h_min;
params.filter_h_max = params_config_.filter_threshold.h_max;
params.filter_s_min = params_config_.filter_threshold.s_min;
params.filter_s_max = params_config_.filter_threshold.s_max;
params.filter_v_min = params_config_.filter_threshold.v_min;
params.filter_v_max = params_config_.filter_threshold.v_max;
params.ellipse_size = params_config_.ellipse_size;
param_pub_.publish(params);
}
void BallDetector::printConfig()
{
if (init_param_ == false)

3
ball_detector/srv/GetParameters.srv Normal file
View File

@ -0,0 +1,3 @@
---
BallDetectorParams returns

3
ball_detector/srv/SetParameters.srv Normal file
View File

@ -0,0 +1,3 @@
BallDetectorParams params
---
BallDetectorParams returns

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

@ -55,6 +55,7 @@ class ActionDemo : public OPDemo
PlayAction = 1,
PauseAction = 2,
StopAction = 3,
ReadyAction = 4,
};
const int SPIN_RATE;
@ -88,12 +89,14 @@ class ActionDemo : public OPDemo
void actionSetNameCallback(const std_msgs::String::ConstPtr& msg);
void buttonHandlerCallback(const std_msgs::String::ConstPtr& msg);
void demoCommandCallback(const std_msgs::String::ConstPtr &msg);
ros::Publisher module_control_pub_;
ros::Publisher motion_index_pub_;
ros::Publisher play_sound_pub_;
ros::Subscriber buttuon_sub_;
ros::Subscriber demo_command_sub_;
ros::ServiceClient is_running_client_;

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

@ -56,6 +56,7 @@ class VisionDemo : public OPDemo
void buttonHandlerCallback(const std_msgs::String::ConstPtr& msg);
void facePositionCallback(const std_msgs::Int32MultiArray::ConstPtr &msg);
void demoCommandCallback(const std_msgs::String::ConstPtr &msg);
void setModuleToDemo(const std::string &module_name);

5
op3_demo/launch/demo.launch
View File

@ -16,7 +16,10 @@
<!-- 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 demo -->
<node pkg="op3_demo" type="op_demo_node" name="op3_demo" output="screen">
<param name="grass_demo" type="bool" value="False" />

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

@ -37,6 +37,8 @@ ActionDemo::ActionDemo()
std::string default_play_list = "default";
play_list_name_ = nh.param<std::string>("action_script_play_list", default_play_list);
demo_command_sub_ = nh.subscribe("/robotis/demo_command", 1, &ActionDemo::demoCommandCallback, this);
parseActionScript (script_path_);
boost::thread queue_thread = boost::thread(boost::bind(&ActionDemo::callbackThread, this));
@ -51,7 +53,7 @@ void ActionDemo::setDemoEnable()
{
setModuleToDemo("action_module");
usleep(10 * 1000);
usleep(100 * 1000);
enable_ = true;
@ -108,6 +110,8 @@ void ActionDemo::process()
stopMP3();
brakeAction();
play_status_ = ReadyAction;
break;
}
@ -116,6 +120,8 @@ void ActionDemo::process()
stopMP3();
stopAction();
play_status_ = ReadyAction;
break;
}
@ -360,4 +366,19 @@ void ActionDemo::setModuleToDemo(const std::string &module_name)
std::cout << "enable module : " << module_name << std::endl;
}
void ActionDemo::demoCommandCallback(const std_msgs::String::ConstPtr &msg)
{
if (enable_ == false)
return;
if (msg->data == "start")
{
resumeProcess();
}
else if (msg->data == "stop")
{
pauseProcess();
}
}
} /* namespace robotis_op */

55
op3_demo/src/demo_node.cpp
View File

@ -41,6 +41,8 @@ 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 = false;
@ -73,6 +75,7 @@ int main(int argc, char **argv)
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/";
@ -313,3 +316,55 @@ void dxlTorqueChecker()
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);
}
}
}

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

@ -42,7 +42,7 @@ SoccerDemo::SoccerDemo()
ros::NodeHandle nh(ros::this_node::getName());
std::string default_path = ros::package::getPath("op3_gui_demo") + "/config/demo_config.yaml";
std::string default_path = ros::package::getPath("op3_gui_demo") + "/config/gui_config.yaml";
std::string path = nh.param<std::string>("demo_config", default_path);
parseJointNameFromYaml(path);
@ -208,7 +208,7 @@ void SoccerDemo::callbackThread()
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);
demo_command_sub_ = nh.subscribe("/robotis/demo_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");
@ -414,7 +414,7 @@ void SoccerDemo::startSoccerMode()
{
setModuleToDemo("action_module");
usleep(10 * 1000);
usleep(100 * 1000);
playMotion(WalkingReady);
@ -422,7 +422,7 @@ void SoccerDemo::startSoccerMode()
setBodyModuleToDemo("walking_module");
usleep(10 * 1000);
usleep(20 * 1000);
ROS_INFO("Start Soccer Demo");
on_following_ball_ = true;

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

@ -52,7 +52,7 @@ void VisionDemo::setDemoEnable()
setModuleToDemo("head_control_module");
usleep(10 * 1000);
usleep(20 * 1000);
enable_ = true;
face_tracker_.startTracking();
@ -141,29 +141,7 @@ void VisionDemo::buttonHandlerCallback(const std_msgs::String::ConstPtr& msg)
if (msg->data == "start")
{
// switch (play_status_)
// {
// case PlayAction:
// {
// pauseProcess();
// break;
// }
//
// case PauseAction:
// {
// resumeProcess();
// break;
// }
//
// case StopAction:
// {
// resumeProcess();
// break;
// }
//
// default:
// break;
// }
}
else if (msg->data == "mode")
{
@ -171,6 +149,21 @@ void VisionDemo::buttonHandlerCallback(const std_msgs::String::ConstPtr& msg)
}
}
void VisionDemo::demoCommandCallback(const std_msgs::String::ConstPtr &msg)
{
if (enable_ == false)
return;
if (msg->data == "start")
{
}
else if (msg->data == "stop")
{
}
}
void VisionDemo::setModuleToDemo(const std::string &module_name)
{
std_msgs::String control_msg;