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

latest pushes

Browse Source
This commit is contained in:
Ronaldson Bellande 2023-11-01 14:27:16 -04:00
parent c9febaff91
commit 353d812113
9 changed files with 55 additions and 36 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
humanoid_robot_demo/src/action/action_demo.cpp
View File

@ -158,8 +158,9 @@ void ActionDemo::callbackThread() {
buttuon_sub_ = nh.subscribe("/humanoid_robot/open_cr/button", 1,
&ActionDemo::buttonHandlerCallback, this);
is_running_client_ = nh.serviceClient<humanoid_robot_action_module_msgs::IsRunning>(
"/humanoid_robot/action/is_running");
is_running_client_ =
nh.serviceClient<humanoid_robot_action_module_msgs::IsRunning>(
"/humanoid_robot/action/is_running");
set_joint_module_client_ =
nh.serviceClient<humanoid_robot_controller_msgs::SetModule>(
"/humanoid_robot/set_present_ctrl_modules");

20
humanoid_robot_demo/src/demo_node.cpp
View File

@ -19,10 +19,10 @@
#include <ros/ros.h>
#include <std_msgs/String.h>
#include "humanoid_robot_controller_msgs/SyncWriteItem.h"
#include "humanoid_robot_demo/action_demo.h"
#include "humanoid_robot_demo/soccer_demo.h"
#include "humanoid_robot_demo/vision_demo.h"
#include "humanoid_robot_controller_msgs/SyncWriteItem.h"
#include "humanoid_robot_math/humanoid_robot_linear_algebra.h"
enum Demo_Status {
@ -62,23 +62,29 @@ int main(int argc, char **argv) {
// create ros wrapper object
humanoid_robot_op::OPDemo *current_demo = NULL;
humanoid_robot_op::SoccerDemo *soccer_demo = new humanoid_robot_op::SoccerDemo();
humanoid_robot_op::ActionDemo *action_demo = new humanoid_robot_op::ActionDemo();
humanoid_robot_op::VisionDemo *vision_demo = new humanoid_robot_op::VisionDemo();
humanoid_robot_op::SoccerDemo *soccer_demo =
new humanoid_robot_op::SoccerDemo();
humanoid_robot_op::ActionDemo *action_demo =
new humanoid_robot_op::ActionDemo();
humanoid_robot_op::VisionDemo *vision_demo =
new humanoid_robot_op::VisionDemo();
ros::NodeHandle nh(ros::this_node::getName());
init_pose_pub = nh.advertise<std_msgs::String>("/humanoid_robot/base/ini_pose", 0);
init_pose_pub =
nh.advertise<std_msgs::String>("/humanoid_robot/base/ini_pose", 0);
play_sound_pub = nh.advertise<std_msgs::String>("/play_sound_file", 0);
led_pub = nh.advertise<humanoid_robot_controller_msgs::SyncWriteItem>(
"/humanoid_robot/sync_write_item", 0);
dxl_torque_pub = nh.advertise<std_msgs::String>("/humanoid_robot/dxl_torque", 0);
dxl_torque_pub =
nh.advertise<std_msgs::String>("/humanoid_robot/dxl_torque", 0);
ros::Subscriber buttuon_sub =
nh.subscribe("/humanoid_robot/open_cr/button", 1, buttonHandlerCallback);
ros::Subscriber mode_command_sub =
nh.subscribe("/humanoid_robot/mode_command", 1, demoModeCommandCallback);
default_mp3_path = ros::package::getPath("humanoid_robot_demo") + "/data/mp3/";
default_mp3_path =
ros::package::getPath("humanoid_robot_demo") + "/data/mp3/";
ros::start();

7
humanoid_robot_demo/src/soccer/ball_follower.cpp
View File

@ -39,8 +39,9 @@ BallFollower::BallFollower()
set_walking_command_pub_ =
nh_.advertise<std_msgs::String>("/humanoid_robot/walking/command", 0);
set_walking_param_pub_ = nh_.advertise<humanoid_robot_walking_module_msgs::WalkingParam>(
"/humanoid_robot/walking/set_params", 0);
set_walking_param_pub_ =
nh_.advertise<humanoid_robot_walking_module_msgs::WalkingParam>(
"/humanoid_robot/walking/set_params", 0);
get_walking_param_client_ =
nh_.serviceClient<humanoid_robot_walking_module_msgs::GetWalkingParam>(
"/humanoid_robot/walking/get_params");
@ -196,7 +197,7 @@ bool BallFollower::processFollowing(double x_angle, double y_angle,
<< (y_angle * 180 / M_PI));
ROS_INFO_STREAM_COND(DEBUG_PRINT, "foot to kick : " << ball_x_angle);
ROS_INFO("In range [%d | %d]", count_to_kick_, ball_x_angle);
ROS_INFO("In range [%d | %f]", count_to_kick_, ball_x_angle);
// ball queue
// if(ball_position_queue_.size() >= 5)

4
humanoid_robot_demo/src/soccer/ball_tracker.cpp
View File

@ -39,8 +39,8 @@ BallTracker::BallTracker()
"/humanoid_robot/head_control/set_joint_states_offset", 0);
head_joint_pub_ = nh_.advertise<sensor_msgs::JointState>(
"/humanoid_robot/head_control/set_joint_states", 0);
head_scan_pub_ =
nh_.advertise<std_msgs::String>("/humanoid_robot/head_control/scan_command", 0);
head_scan_pub_ = nh_.advertise<std_msgs::String>(
"/humanoid_robot/head_control/scan_command", 0);
// error_pub_ =
// nh_.advertise<std_msgs::Float64MultiArray>("/ball_tracker/errors", 0);

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

@ -32,8 +32,8 @@ SoccerDemo::SoccerDemo()
ros::NodeHandle nh(ros::this_node::getName());
std::string default_path =
ros::package::getPath("humanoid_robot_gui_demo") + "/config/gui_config.yaml";
std::string default_path = ros::package::getPath("humanoid_robot_gui_demo") +
"/config/gui_config.yaml";
std::string path = nh.param<std::string>("demo_config", default_path);
parseJointNameFromYaml(path);
@ -166,8 +166,9 @@ void SoccerDemo::callbackThread() {
ros::NodeHandle nh(ros::this_node::getName());
// subscriber & publisher
module_control_pub_ = nh.advertise<humanoid_robot_controller_msgs::JointCtrlModule>(
"/humanoid_robot/set_joint_ctrl_modules", 0);
module_control_pub_ =
nh.advertise<humanoid_robot_controller_msgs::JointCtrlModule>(
"/humanoid_robot/set_joint_ctrl_modules", 0);
motion_index_pub_ =
nh.advertise<std_msgs::Int32>("/humanoid_robot/action/page_num", 0);
rgb_led_pub_ = nh.advertise<humanoid_robot_controller_msgs::SyncWriteItem>(
@ -180,8 +181,9 @@ void SoccerDemo::callbackThread() {
imu_data_sub_ = nh.subscribe("/humanoid_robot/open_cr/imu", 1,
&SoccerDemo::imuDataCallback, this);
is_running_client_ = nh.serviceClient<humanoid_robot_action_module_msgs::IsRunning>(
"/humanoid_robot/action/is_running");
is_running_client_ =
nh.serviceClient<humanoid_robot_action_module_msgs::IsRunning>(
"/humanoid_robot/action/is_running");
set_joint_module_client_ =
nh.serviceClient<humanoid_robot_controller_msgs::SetJointModule>(
"/humanoid_robot/set_present_joint_ctrl_modules");

3
humanoid_robot_demo/src/test/button_test.cpp
View File

@ -30,7 +30,8 @@ ButtonTest::ButtonTest() : SPIN_RATE(30), led_count_(0), rgb_led_count_(0) {
boost::thread process_thread =
boost::thread(boost::bind(&ButtonTest::processThread, this));
default_mp3_path_ = ros::package::getPath("humanoid_robot_demo") + "/data/mp3/test/";
default_mp3_path_ =
ros::package::getPath("humanoid_robot_demo") + "/data/mp3/test/";
}
ButtonTest::~ButtonTest() {}

7
humanoid_robot_demo/src/test/mic_test.cpp
View File

@ -32,9 +32,10 @@ MicTest::MicTest()
boost::thread process_thread =
boost::thread(boost::bind(&MicTest::processThread, this));
recording_file_name_ =
ros::package::getPath("humanoid_robot_demo") + "/data/mp3/test/mic-test.wav";
default_mp3_path_ = ros::package::getPath("humanoid_robot_demo") + "/data/mp3/test/";
recording_file_name_ = ros::package::getPath("humanoid_robot_demo") +
"/data/mp3/test/mic-test.wav";
default_mp3_path_ =
ros::package::getPath("humanoid_robot_demo") + "/data/mp3/test/";
start_time_ = ros::Time::now();
}

23
humanoid_robot_demo/src/test_node.cpp
View File

@ -20,12 +20,12 @@
#include <ros/ros.h>
#include <std_msgs/String.h>
#include "humanoid_robot_controller_msgs/SyncWriteItem.h"
#include "humanoid_robot_demo/action_demo.h"
#include "humanoid_robot_demo/button_test.h"
#include "humanoid_robot_demo/mic_test.h"
#include "humanoid_robot_demo/soccer_demo.h"
#include "humanoid_robot_demo/vision_demo.h"
#include "humanoid_robot_controller_msgs/SyncWriteItem.h"
#include "humanoid_robot_math/humanoid_robot_linear_algebra.h"
enum Demo_Status {
@ -67,25 +67,32 @@ int main(int argc, char **argv) {
// create ros wrapper object
humanoid_robot_op::OPDemo *current_demo = NULL;
humanoid_robot_op::SoccerDemo *soccer_demo = new humanoid_robot_op::SoccerDemo();
humanoid_robot_op::ActionDemo *action_demo = new humanoid_robot_op::ActionDemo();
humanoid_robot_op::VisionDemo *vision_demo = new humanoid_robot_op::VisionDemo();
humanoid_robot_op::ButtonTest *button_test = new humanoid_robot_op::ButtonTest();
humanoid_robot_op::SoccerDemo *soccer_demo =
new humanoid_robot_op::SoccerDemo();
humanoid_robot_op::ActionDemo *action_demo =
new humanoid_robot_op::ActionDemo();
humanoid_robot_op::VisionDemo *vision_demo =
new humanoid_robot_op::VisionDemo();
humanoid_robot_op::ButtonTest *button_test =
new humanoid_robot_op::ButtonTest();
humanoid_robot_op::MicTest *mic_test = new humanoid_robot_op::MicTest();
ros::NodeHandle nh(ros::this_node::getName());
init_pose_pub = nh.advertise<std_msgs::String>("/humanoid_robot/base/ini_pose", 0);
init_pose_pub =
nh.advertise<std_msgs::String>("/humanoid_robot/base/ini_pose", 0);
play_sound_pub = nh.advertise<std_msgs::String>("/play_sound_file", 0);
led_pub = nh.advertise<humanoid_robot_controller_msgs::SyncWriteItem>(
"/humanoid_robot/sync_write_item", 0);
dxl_torque_pub = nh.advertise<std_msgs::String>("/humanoid_robot/dxl_torque", 0);
dxl_torque_pub =
nh.advertise<std_msgs::String>("/humanoid_robot/dxl_torque", 0);
ros::Subscriber buttuon_sub =
nh.subscribe("/humanoid_robot/open_cr/button", 1, buttonHandlerCallback);
ros::Subscriber mode_command_sub =
nh.subscribe("/humanoid_robot/mode_command", 1, demoModeCommandCallback);
default_mp3_path = ros::package::getPath("humanoid_robot_demo") + "/data/mp3/";
default_mp3_path =
ros::package::getPath("humanoid_robot_demo") + "/data/mp3/";
ros::start();

8
humanoid_robot_demo/src/vision/face_tracker.cpp
View File

@ -28,13 +28,13 @@ FaceTracker::FaceTracker()
"/humanoid_robot/head_control/set_joint_states_offset", 0);
head_joint_pub_ = nh_.advertise<sensor_msgs::JointState>(
"/humanoid_robot/head_control/set_joint_states", 0);
head_scan_pub_ =
nh_.advertise<std_msgs::String>("/humanoid_robot/head_control/scan_command", 0);
head_scan_pub_ = nh_.advertise<std_msgs::String>(
"/humanoid_robot/head_control/scan_command", 0);
face_position_sub_ = nh_.subscribe("/face_position", 1,
&FaceTracker::facePositionCallback, this);
// face_tracking_command_sub_ = nh_.subscribe("/humanoid_robot/demo_command", 1,
// &FaceTracker::faceTrackerCommandCallback, this);
// face_tracking_command_sub_ = nh_.subscribe("/humanoid_robot/demo_command",
// 1, &FaceTracker::faceTrackerCommandCallback, this);
}
FaceTracker::~FaceTracker() {}