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

changed a method to setting module

Browse Source 
added parameters of gain for ball_tracking
changed variables of camera FOV
This commit is contained in:
Kayman
2018-03-22 16:33:10 +09:00
parent fe3117af0c
commit e5503b7544
14 changed files with 308 additions and 163 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

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

4
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,6 +60,7 @@ 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_;
@ -70,9 +72,9 @@ class VisionDemo : public OPDemo
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>

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)

93
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;
}
@ -205,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)
{
@ -236,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;
@ -259,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")
{
@ -299,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);
}
}
}
@ -344,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);
}
}
}

2
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),

31
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,18 +43,15 @@ 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);
@ -70,7 +66,6 @@ void VisionDemo::setDemoDisable()
{
face_tracker_.stopTracking();
is_tracking_ = false;
tracking_status_ = FaceTracker::Waiting;
enable_ = false;
@ -102,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()
@ -136,6 +128,8 @@ void VisionDemo::callbackThread()
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();
@ -176,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)