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

added that a head goes init when OP3 stop tracking the ball.

Browse Source
This commit is contained in:
Kayman 2018-09-07 17:36:15 +09:00
parent 169cfefcca
commit ebaf7f9e21
3 changed files with 21 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -56,6 +56,7 @@ public:
void stopTracking();
void setUsingHeadScan(bool use_scan);
void goInit();
double getPanOfBall()
{
@ -89,6 +90,7 @@ protected:
//image publisher/subscriber
ros::Publisher module_control_pub_;
ros::Publisher head_joint_offset_pub_;
ros::Publisher head_joint_pub_;
ros::Publisher head_scan_pub_;

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

@ -45,7 +45,8 @@ BallTracker::BallTracker()
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_joint_offset_pub_ = nh_.advertise<sensor_msgs::JointState>("/robotis/head_control/set_joint_states_offset", 0);
head_joint_pub_ = nh_.advertise<sensor_msgs::JointState>("/robotis/head_control/set_joint_states", 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);
@ -97,6 +98,8 @@ void BallTracker::startTracking()
void BallTracker::stopTracking()
{
goInit();
on_tracking_ = false;
ROS_INFO_COND(DEBUG_PRINT, "Stop Ball tracking");
@ -246,6 +249,19 @@ void BallTracker::publishHeadJoint(double pan, double tilt)
head_angle_msg.position.push_back(pan);
head_angle_msg.position.push_back(tilt);
head_joint_offset_pub_.publish(head_angle_msg);
}
void BallTracker::goInit()
{
sensor_msgs::JointState head_angle_msg;
head_angle_msg.name.push_back("head_pan");
head_angle_msg.name.push_back("head_tilt");
head_angle_msg.position.push_back(0.0);
head_angle_msg.position.push_back(0.0);
head_joint_pub_.publish(head_angle_msg);
}

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

@ -497,12 +497,12 @@ void SoccerDemo::stopSoccerMode()
void SoccerDemo::handleKick(int ball_position)
{
usleep(1000 * 1000);
usleep(1500 * 1000);
// change to motion module
setModuleToDemo("action_module");
usleep(1500 * 1000);
//usleep(1500 * 1000);
if (handleFallen(stand_state_) == true || enable_ == false)
return;