From 464ef98717b9f3732f5fadf95e29e4dae1aca0ea Mon Sep 17 00:00:00 2001 From: Kayman Date: Thu, 13 Jul 2017 16:43:12 +0900 Subject: [PATCH] cleanup unused files. modified tracking gains. --- op3_demo/CMakeLists.txt | 25 -- op3_demo/include/op3_demo/ball_follower.h | 5 +- op3_demo/include/op3_demo/op_demo.h | 1 + op3_demo/include/op3_demo/soccer_demo.h | 1 + op3_demo/launch/ball_tracker.launch | 10 - op3_demo/launch/demo.launch | 8 +- op3_demo/launch/self_test.launch | 4 +- op3_demo/src/ball_tracker_node.cpp | 133 ------ op3_demo/src/demo_node.cpp | 3 + op3_demo/src/soccer/ball_follower.cpp | 85 ++-- op3_demo/src/soccer/ball_tracker.cpp | 7 +- op3_demo/src/soccer/soccer_demo.cpp | 14 +- op3_demo/src/soccer_demo_node.cpp | 504 ---------------------- 13 files changed, 76 insertions(+), 724 deletions(-) delete mode 100644 op3_demo/launch/ball_tracker.launch delete mode 100644 op3_demo/src/ball_tracker_node.cpp delete mode 100644 op3_demo/src/soccer_demo_node.cpp diff --git a/op3_demo/CMakeLists.txt b/op3_demo/CMakeLists.txt index 1a1913c..dc1fdea 100644 --- a/op3_demo/CMakeLists.txt +++ b/op3_demo/CMakeLists.txt @@ -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 diff --git a/op3_demo/include/op3_demo/ball_follower.h b/op3_demo/include/op3_demo/ball_follower.h index bc7f97b..9921056 100644 --- a/op3_demo/include/op3_demo/ball_follower.h +++ b/op3_demo/include/op3_demo/ball_follower.h @@ -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_; diff --git a/op3_demo/include/op3_demo/op_demo.h b/op3_demo/include/op3_demo/op_demo.h index 55e1785..938091a 100644 --- a/op3_demo/include/op3_demo/op_demo.h +++ b/op3_demo/include/op3_demo/op_demo.h @@ -48,6 +48,7 @@ class OPDemo RightKick = 121, LeftKick = 120, Ceremony = 85, + ForGrass = 20, }; OPDemo() diff --git a/op3_demo/include/op3_demo/soccer_demo.h b/op3_demo/include/op3_demo/soccer_demo.h index c7cf790..38abd9a 100644 --- a/op3_demo/include/op3_demo/soccer_demo.h +++ b/op3_demo/include/op3_demo/soccer_demo.h @@ -111,6 +111,7 @@ class SoccerDemo : public OPDemo std::map id_joint_table_; std::map joint_id_table_; + bool is_grass_; int wait_count_; bool on_following_ball_; bool restart_soccer_; diff --git a/op3_demo/launch/ball_tracker.launch b/op3_demo/launch/ball_tracker.launch deleted file mode 100644 index 94126b5..0000000 --- a/op3_demo/launch/ball_tracker.launch +++ /dev/null @@ -1,10 +0,0 @@ - - - - - - - - - - diff --git a/op3_demo/launch/demo.launch b/op3_demo/launch/demo.launch index 15ce10d..c96b5b9 100644 --- a/op3_demo/launch/demo.launch +++ b/op3_demo/launch/demo.launch @@ -2,7 +2,7 @@ - + @@ -17,7 +17,9 @@ - - + + + + diff --git a/op3_demo/launch/self_test.launch b/op3_demo/launch/self_test.launch index dea379d..0f34bca 100644 --- a/op3_demo/launch/self_test.launch +++ b/op3_demo/launch/self_test.launch @@ -2,7 +2,7 @@ - + @@ -17,7 +17,7 @@ - + diff --git a/op3_demo/src/ball_tracker_node.cpp b/op3_demo/src/ball_tracker_node.cpp deleted file mode 100644 index 62ed643..0000000 --- a/op3_demo/src/ball_tracker_node.cpp +++ /dev/null @@ -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("/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") - { - - } - -} diff --git a/op3_demo/src/demo_node.cpp b/op3_demo/src/demo_node.cpp index f37a09f..0ab4bae 100644 --- a/op3_demo/src/demo_node.cpp +++ b/op3_demo/src/demo_node.cpp @@ -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) { diff --git a/op3_demo/src/soccer/ball_follower.cpp b/op3_demo/src/soccer/ball_follower.cpp index e6192ed..18f0524 100644 --- a/op3_demo/src/soccer/ball_follower.cpp +++ b/op3_demo/src/soccer/ball_follower.cpp @@ -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; } diff --git a/op3_demo/src/soccer/ball_tracker.cpp b/op3_demo/src/soccer/ball_tracker.cpp index 6584bd8..da8ade2 100644 --- a/op3_demo/src/soccer/ball_tracker.cpp +++ b/op3_demo/src/soccer/ball_tracker.cpp @@ -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; diff --git a/op3_demo/src/soccer/soccer_demo.cpp b/op3_demo/src/soccer/soccer_demo.cpp index 6b3bfde..675e094 100644 --- a/op3_demo/src/soccer/soccer_demo.cpp +++ b/op3_demo/src/soccer/soccer_demo.cpp @@ -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("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: diff --git a/op3_demo/src/soccer_demo_node.cpp b/op3_demo/src/soccer_demo_node.cpp deleted file mode 100644 index 76d1f39..0000000 --- a/op3_demo/src/soccer_demo_node.cpp +++ /dev/null @@ -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 -#include -#include -#include - -#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 id_joint_table; -std::map 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("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/set_joint_ctrl_modules", 0); - motion_index_pub = nh.advertise("/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(); - _joint_name = _it->second.as(); - - 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::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::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); -}