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

cleanup unused files.

Browse Source 
modified tracking gains.
This commit is contained in:
Kayman 2017-07-13 16:43:12 +09:00
parent 288370a8dc
commit 464ef98717
13 changed files with 76 additions and 724 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

25
op3_demo/CMakeLists.txt
View File

@ -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

5
op3_demo/include/op3_demo/ball_follower.h
View File

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

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

@ -48,6 +48,7 @@ class OPDemo
RightKick = 121,
LeftKick = 120,
Ceremony = 85,
ForGrass = 20,
};
OPDemo()

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

@ -111,6 +111,7 @@ class SoccerDemo : public OPDemo
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 restart_soccer_;

10
op3_demo/launch/ball_tracker.launch
View File

@ -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>

8
op3_demo/launch/demo.launch
View File

@ -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,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>

4
op3_demo/launch/self_test.launch
View File

@ -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>

133
op3_demo/src/ball_tracker_node.cpp
View File

@ -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")
{
}
}

3
op3_demo/src/demo_node.cpp
View File

@ -189,6 +189,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)
{

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

@ -46,11 +46,11 @@ BallFollower::BallFollower()
kick_motion_index_(83),
CAMERA_HEIGHT(0.46),
NOT_FOUND_THRESHOLD(50),
MAX_FB_STEP(25.0 * 0.001),
MAX_FB_STEP(35.0 * 0.001),
MAX_RL_TURN(15.0 * M_PI / 180),
MIN_FB_STEP(5.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),
@ -139,6 +139,44 @@ void BallFollower::currentJointStatesCallback(const sensor_msgs::JointState::Con
current_tilt_ = 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)
bool BallFollower::processFollowing(double x_angle, double y_angle, double ball_size)
{
@ -169,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;
@ -178,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))
@ -232,48 +269,16 @@ bool BallFollower::processFollowing(double x_angle, double y_angle, double ball_
accum_ball_position_ = 0;
}
double next_movement = current_x_move_;
double fb_move = 0.0, rl_angle = 0.0;
double distance_to_walk = distance_to_ball - distance_to_kick;
if(distance_to_walk < 0)
distance_to_walk = 0.0;
fb_goal = fmin(distance_to_walk * 0.1, MAX_FB_STEP);
accum_period_time_ += delta_time;
if (accum_period_time_ > (curr_period_time_ * 0.25))
{
accum_period_time_ = 0.0;
if ((distance_to_walk * 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", distance_to_ball, fb_move,
delta_time);
ROS_INFO_COND(DEBUG_PRINT, "==============================================");
// calc rl angle
double rl_goal = 0.0, rl_angle = 0.0;
if (fabs(current_pan_) * 180 / M_PI > 5.0)
{
double rl_offset = fabs(current_pan_) * 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 (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);
//ROS_INFO("distance to ball : %6.4f, fb : %6.4f, delta : %6.6f", distance_to_ball, fb_move, delta_time);
return false;
}

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

@ -101,6 +101,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)
@ -150,7 +154,8 @@ 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.75, d_gain = 0.05;
double p_gain = 0.7, d_gain = 0.05;
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;

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

@ -61,6 +61,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()
@ -103,12 +105,16 @@ void SoccerDemo::process()
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)
{
@ -432,12 +438,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:
@ -473,12 +479,12 @@ 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:

504
op3_demo/src/soccer_demo_node.cpp
View File

@ -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);
}