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

Merge pull request #24 from RonaldsonBellande/main

Browse Source 
Update
This commit is contained in:
Ronaldson Bellande 2024-06-26 01:17:27 -04:00 committed by GitHub
commit d463950960
No known key found for this signature in database
GPG Key ID: B5690EEEBB952194
5 changed files with 214 additions and 313 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

54
humanoid_robot_intelligence_control_system_read_write_demo/CMakeLists.txt
View File

@ -25,7 +25,13 @@ if($ENV{ROS_VERSION} EQUAL 1)
std_msgs
)
else()
find_package(ament_cmake REQUIRED)
find_package(
ament_cmake REQUIRED COMPONENTS
humanoid_robot_intelligence_control_system_controller_msgs
roscpp
sensor_msgs
std_msgs
)
endif()
@ -41,31 +47,23 @@ if($ENV{ROS_VERSION} EQUAL 1)
)
endif()
# Install Python scripts for both ROS 1
if($ENV{ROS_VERSION} EQUAL 1)
catkin_install_python(
PROGRAMS
src/read_write.py
DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)
endif()
include_directories(${catkin_INCLUDE_DIRS})
add_executable(
read_write
src/read_write.cpp
)
add_dependencies(
read_write
${${PROJECT_NAME}_EXPORTED_TARGETS}
${catkin_EXPORTED_TARGETS}
)
target_link_libraries(
read_write
${catkin_LIBRARIES}
)
install(
TARGETS read_write
RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)
install(
DIRECTORY launch
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
)
# Install Python scripts, configuration files, and launch files
if($ENV{ROS_VERSION} EQUAL "1")
install(PROGRAMS src/read_write.py DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION})
install(DIRECTORY config/ DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/config)
install(DIRECTORY launch/ DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch)
else()
install(PROGRAMS src/read_write.py DESTINATION lib/${PROJECT_NAME})
install(DIRECTORY config/ DESTINATION share/${PROJECT_NAME}/config)
install(DIRECTORY launch/ DESTINATION share/${PROJECT_NAME}/launch)
ament_package()
endif()

12
humanoid_robot_intelligence_control_system_read_write_demo/package.xml
View File

@ -28,6 +28,7 @@ the License.
<buildtool_depend condition="$ROS_VERSION == 1">catkin</buildtool_depend>
<build_depend condition="$ROS_VERSION == 1">roscpp</build_depend>
<build_depend condition="$ROS_VERSION == 1">rospy</build_depend>
<build_depend condition="$ROS_VERSION == 1">std_msgs</build_depend>
<build_depend condition="$ROS_VERSION == 1">sensor_msgs</build_depend>
<build_depend condition="$ROS_VERSION == 1">
@ -35,6 +36,7 @@ the License.
<build_depend condition="$ROS_VERSION == 1">humanoid_robot_intelligence_control_system_manager</build_depend>
<build_export_depend condition="$ROS_VERSION == 1">roscpp</build_export_depend>
<build_export_depend condition="$ROS_VERSION == 1">rospy</build_export_depend>
<build_export_depend condition="$ROS_VERSION == 1">std_msgs</build_export_depend>
<build_export_depend condition="$ROS_VERSION == 1">sensor_msgs</build_export_depend>
<build_export_depend condition="$ROS_VERSION == 1">
@ -43,6 +45,7 @@ the License.
humanoid_robot_intelligence_control_system_manager</build_export_depend>
<exec_depend condition="$ROS_VERSION == 1">roscpp</exec_depend>
<exec_depend condition="$ROS_VERSION == 1">rospy</exec_depend>
<exec_depend condition="$ROS_VERSION == 1">std_msgs</exec_depend>
<exec_depend condition="$ROS_VERSION == 1">sensor_msgs</exec_depend>
<exec_depend condition="$ROS_VERSION == 1">
@ -52,14 +55,16 @@ the License.
<buildtool_depend condition="$ROS_VERSION == 2">ament_cmake</buildtool_depend>
<build_depend condition="$ROS_VERSION == 2">roscpp</build_depend>
<build_depend condition="$ROS_VERSION == 2">rclcpp</build_depend>
<build_depend condition="$ROS_VERSION == 2">rclpy</build_depend>
<build_depend condition="$ROS_VERSION == 2">std_msgs</build_depend>
<build_depend condition="$ROS_VERSION == 2">sensor_msgs</build_depend>
<build_depend condition="$ROS_VERSION == 2">
humanoid_robot_intelligence_control_system_controller_msgs</build_depend>
<build_depend condition="$ROS_VERSION == 2">humanoid_robot_intelligence_control_system_manager</build_depend>
<build_export_depend condition="$ROS_VERSION == 2">roscpp</build_export_depend>
<build_export_depend condition="$ROS_VERSION == 2">rclcpp</build_export_depend>
<build_export_depend condition="$ROS_VERSION == 2">rclpy</build_export_depend>
<build_export_depend condition="$ROS_VERSION == 2">std_msgs</build_export_depend>
<build_export_depend condition="$ROS_VERSION == 2">sensor_msgs</build_export_depend>
<build_export_depend condition="$ROS_VERSION == 2">
@ -67,7 +72,8 @@ the License.
<build_export_depend condition="$ROS_VERSION == 2">
humanoid_robot_intelligence_control_system_manager</build_export_depend>
<exec_depend condition="$ROS_VERSION == 2">roscpp</exec_depend>
<exec_depend condition="$ROS_VERSION == 2">rclcpp</exec_depend>
<exec_depend condition="$ROS_VERSION == 2">rclpy</exec_depend>
<exec_depend condition="$ROS_VERSION == 2">std_msgs</exec_depend>
<exec_depend condition="$ROS_VERSION == 2">sensor_msgs</exec_depend>
<exec_depend condition="$ROS_VERSION == 2">

11
humanoid_robot_intelligence_control_system_read_write_demo/setup.py Normal file
View File

@ -0,0 +1,11 @@
from distutils.core import setup
from catkin_pkg.python_setup import generate_distutils_setup
# fetch values from package.xml
setup_args = generate_distutils_setup(
scripts=['src/read_write.py'],
packages=['humanoid_robot_intelligence_control_system_read_write_demo'],
package_dir={'': 'src'},
)
setup(**setup_args)

282
humanoid_robot_intelligence_control_system_read_write_demo/src/read_write.cpp
View File

@ -1,282 +0,0 @@
/*******************************************************************************
* Copyright 2017 ROBOTIS CO., LTD.
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*******************************************************************************/
/* Author: Kayman Jung */
#include <ros/ros.h>
#include <sensor_msgs/JointState.h>
#include <std_msgs/String.h>
#include "humanoid_robot_intelligence_control_system_controller_msgs/SetModule.h"
#include "humanoid_robot_intelligence_control_system_controller_msgs/SyncWriteItem.h"
void buttonHandlerCallback(const std_msgs::String::ConstPtr &msg);
void jointstatesCallback(const sensor_msgs::JointState::ConstPtr &msg);
void readyToDemo();
void setModule(const std::string &module_name);
void goInitPose();
void setLED(int led);
bool checkManagerRunning(std::string &manager_name);
void torqueOnAll();
void torqueOff(const std::string &body_side);
enum ControlModule {
None = 0,
DirectControlModule = 1,
Framework = 2,
};
const int SPIN_RATE = 30;
const bool DEBUG_PRINT = false;
ros::Publisher init_pose_pub;
ros::Publisher sync_write_pub;
ros::Publisher dxl_torque_pub;
ros::Publisher write_joint_pub;
ros::Publisher write_joint_pub2;
ros::Subscriber buttuon_sub;
ros::Subscriber read_joint_sub;
ros::ServiceClient set_joint_module_client;
int control_module = None;
bool demo_ready = false;
// node main
int main(int argc, char **argv) {
// init ros
ros::init(argc, argv, "read_write");
ros::NodeHandle nh(ros::this_node::getName());
init_pose_pub = nh.advertise<std_msgs::String>("/humanoid_robot_intelligence_control_system/base/ini_pose", 0);
sync_write_pub = nh.advertise<humanoid_robot_intelligence_control_system_controller_msgs::SyncWriteItem>(
"/humanoid_robot_intelligence_control_system/sync_write_item", 0);
dxl_torque_pub = nh.advertise<std_msgs::String>("/humanoid_robot_intelligence_control_system/dxl_torque", 0);
write_joint_pub =
nh.advertise<sensor_msgs::JointState>("/humanoid_robot_intelligence_control_system/set_joint_states", 0);
write_joint_pub2 = nh.advertise<sensor_msgs::JointState>(
"/humanoid_robot_intelligence_control_system/direct_control/set_joint_states", 0);
read_joint_sub =
nh.subscribe("/humanoid_robot_intelligence_control_system/present_joint_states", 1, jointstatesCallback);
buttuon_sub =
nh.subscribe("/humanoid_robot_intelligence_control_system/open_cr/button", 1, buttonHandlerCallback);
// service
set_joint_module_client =
nh.serviceClient<humanoid_robot_intelligence_control_system_controller_msgs::SetModule>(
"/humanoid_robot_intelligence_control_system/set_present_ctrl_modules");
ros::start();
// set node loop rate
ros::Rate loop_rate(SPIN_RATE);
// wait for starting of humanoid_robot_intelligence_control_system_manager
std::string manager_name = "/humanoid_robot_intelligence_control_system_manager";
while (ros::ok()) {
ros::Duration(1.0).sleep();
if (checkManagerRunning(manager_name) == true) {
break;
ROS_INFO_COND(DEBUG_PRINT, "Succeed to connect");
}
ROS_WARN("Waiting for humanoid_robot_intelligence_control_system manager");
}
readyToDemo();
// node loop
while (ros::ok()) {
// process
// execute pending callbacks
ros::spinOnce();
// relax to fit output rate
loop_rate.sleep();
}
// exit program
return 0;
}
void buttonHandlerCallback(const std_msgs::String::ConstPtr &msg) {
// starting demo using humanoid_robot_intelligence_control_system_controller
if (msg->data == "mode") {
control_module = Framework;
ROS_INFO("Button : mode | Framework");
readyToDemo();
}
// starting demo using direct_control_module
else if (msg->data == "start") {
control_module = DirectControlModule;
ROS_INFO("Button : start | Direct control module");
readyToDemo();
}
// torque on all joints of ROBOTIS-HUMANOID_ROBOT
else if (msg->data == "user") {
torqueOnAll();
control_module = None;
}
}
void jointstatesCallback(const sensor_msgs::JointState::ConstPtr &msg) {
if (control_module == None)
return;
sensor_msgs::JointState write_msg;
write_msg.header = msg->header;
for (int ix = 0; ix < msg->name.size(); ix++) {
std::string joint_name = msg->name[ix];
double joint_position = msg->position[ix];
// mirror and copy joint angles from right to left
if (joint_name == "r_sho_pitch") {
write_msg.name.push_back("r_sho_pitch");
write_msg.position.push_back(joint_position);
write_msg.name.push_back("l_sho_pitch");
write_msg.position.push_back(-joint_position);
}
if (joint_name == "r_sho_roll") {
write_msg.name.push_back("r_sho_roll");
write_msg.position.push_back(joint_position);
write_msg.name.push_back("l_sho_roll");
write_msg.position.push_back(-joint_position);
}
if (joint_name == "r_el") {
write_msg.name.push_back("r_el");
write_msg.position.push_back(joint_position);
write_msg.name.push_back("l_el");
write_msg.position.push_back(-joint_position);
}
}
// publish a message to set the joint angles
if (control_module == Framework)
write_joint_pub.publish(write_msg);
else if (control_module == DirectControlModule)
write_joint_pub2.publish(write_msg);
}
void readyToDemo() {
ROS_INFO("Start Read-Write Demo");
// turn off LED
setLED(0x04);
torqueOnAll();
ROS_INFO("Torque on All joints");
// send message for going init posture
goInitPose();
ROS_INFO("Go Init pose");
// wait while ROBOTIS-HUMANOID_ROBOT goes to the init posture.
ros::Duration(4.0).sleep();
// turn on R/G/B LED [0x01 | 0x02 | 0x04]
setLED(control_module);
// change the module for demo
if (control_module == Framework) {
setModule("none");
ROS_INFO("Change module to none");
} else if (control_module == DirectControlModule) {
setModule("direct_control_module");
ROS_INFO("Change module to direct_control_module");
} else
return;
// torque off : right arm
torqueOff("right");
ROS_INFO("Torque off");
}
void goInitPose() {
std_msgs::String init_msg;
init_msg.data = "ini_pose";
init_pose_pub.publish(init_msg);
}
void setLED(int led) {
humanoid_robot_intelligence_control_system_controller_msgs::SyncWriteItem syncwrite_msg;
syncwrite_msg.item_name = "LED";
syncwrite_msg.joint_name.push_back("open-cr");
syncwrite_msg.value.push_back(led);
sync_write_pub.publish(syncwrite_msg);
}
bool checkManagerRunning(std::string &manager_name) {
std::vector<std::string> node_list;
ros::master::getNodes(node_list);
for (unsigned int node_list_idx = 0; node_list_idx < node_list.size();
node_list_idx++) {
if (node_list[node_list_idx] == manager_name)
return true;
}
ROS_ERROR("Can't find humanoid_robot_intelligence_control_system_manager");
return false;
}
void setModule(const std::string &module_name) {
humanoid_robot_intelligence_control_system_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 torqueOnAll() {
std_msgs::String check_msg;
check_msg.data = "check";
dxl_torque_pub.publish(check_msg);
}
void torqueOff(const std::string &body_side) {
humanoid_robot_intelligence_control_system_controller_msgs::SyncWriteItem syncwrite_msg;
int torque_value = 0;
syncwrite_msg.item_name = "torque_enable";
if (body_side == "right") {
syncwrite_msg.joint_name.push_back("r_sho_pitch");
syncwrite_msg.value.push_back(torque_value);
syncwrite_msg.joint_name.push_back("r_sho_roll");
syncwrite_msg.value.push_back(torque_value);
syncwrite_msg.joint_name.push_back("r_el");
syncwrite_msg.value.push_back(torque_value);
} else if (body_side == "left") {
syncwrite_msg.joint_name.push_back("l_sho_pitch");
syncwrite_msg.value.push_back(torque_value);
syncwrite_msg.joint_name.push_back("l_sho_roll");
syncwrite_msg.value.push_back(torque_value);
syncwrite_msg.joint_name.push_back("l_el");
syncwrite_msg.value.push_back(torque_value);
} else
return;
sync_write_pub.publish(syncwrite_msg);
}

168
humanoid_robot_intelligence_control_system_read_write_demo/src/read_write.py Normal file
View File

@ -0,0 +1,168 @@
#!/usr/bin/env python
# Copyright (C) 2024 Bellande Robotics Sensors Research Innovation Center, Ronaldson Bellande
#
# Licensed under the Apache License, Version 2.0 (the "License"); you may not
# use this file except in compliance with the License. You may obtain a copy of
# the License at
#
# http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
# License for the specific language governing permissions and limitations under
# the License.
import rospy
from sensor_msgs.msg import JointState
from std_msgs.msg import String
from humanoid_robot_intelligence_control_system_controller_msgs.srv import SetModule
from humanoid_robot_intelligence_control_system_controller_msgs.msg import SyncWriteItem
def button_handler_callback(msg):
global control_module
if msg.data == "mode":
control_module = ControlModule.Framework
rospy.loginfo("Button : mode | Framework")
ready_to_demo()
elif msg.data == "start":
control_module = ControlModule.DirectControlModule
rospy.loginfo("Button : start | Direct control module")
ready_to_demo()
elif msg.data == "user":
torque_on_all()
control_module = ControlModule.None
def joint_states_callback(msg):
if control_module == ControlModule.None:
return
write_msg = JointState()
write_msg.header = msg.header
for ix in range(len(msg.name)):
joint_name = msg.name[ix]
joint_position = msg.position[ix]
if joint_name == "r_sho_pitch":
write_msg.name.append("r_sho_pitch")
write_msg.position.append(joint_position)
write_msg.name.append("l_sho_pitch")
write_msg.position.append(-joint_position)
if joint_name == "r_sho_roll":
write_msg.name.append("r_sho_roll")
write_msg.position.append(joint_position)
write_msg.name.append("l_sho_roll")
write_msg.position.append(-joint_position)
if joint_name == "r_el":
write_msg.name.append("r_el")
write_msg.position.append(joint_position)
write_msg.name.append("l_el")
write_msg.position.append(-joint_position)
if control_module == ControlModule.Framework:
write_joint_pub.publish(write_msg)
elif control_module == ControlModule.DirectControlModule:
write_joint_pub2.publish(write_msg)
def ready_to_demo():
rospy.loginfo("Start Read-Write Demo")
set_led(0x04)
torque_on_all()
rospy.loginfo("Torque on All joints")
go_init_pose()
rospy.loginfo("Go Init pose")
rospy.sleep(4.0)
set_led(control_module)
if control_module == ControlModule.Framework:
set_module("none")
rospy.loginfo("Change module to none")
elif control_module == ControlModule.DirectControlModule:
set_module("direct_control_module")
rospy.loginfo("Change module to direct_control_module")
else:
return
torque_off("right")
rospy.loginfo("Torque off")
def go_init_pose():
init_msg = String()
init_msg.data = "ini_pose"
init_pose_pub.publish(init_msg)
def set_led(led):
syncwrite_msg = SyncWriteItem()
syncwrite_msg.item_name = "LED"
syncwrite_msg.joint_name.append("open-cr")
syncwrite_msg.value.append(led)
sync_write_pub.publish(syncwrite_msg)
def check_manager_running(manager_name):
node_list = rospy.get_published_topics()
for node in node_list:
if node[0] == manager_name:
return True
rospy.logerr("Can't find humanoid_robot_intelligence_control_system_manager")
return False
def set_module(module_name):
try:
set_module_srv = rospy.ServiceProxy('/humanoid_robot_intelligence_control_system/set_present_ctrl_modules', SetModule)
set_module_srv(module_name)
except rospy.ServiceException as e:
rospy.logerr("Failed to set module: %s" % e)
def torque_on_all():
check_msg = String()
check_msg.data = "check"
dxl_torque_pub.publish(check_msg)
def torque_off(body_side):
syncwrite_msg = SyncWriteItem()
torque_value = 0
syncwrite_msg.item_name = "torque_enable"
if body_side == "right":
syncwrite_msg.joint_name.extend(["r_sho_pitch", "r_sho_roll", "r_el"])
syncwrite_msg.value.extend([torque_value] * 3)
elif body_side == "left":
syncwrite_msg.joint_name.extend(["l_sho_pitch", "l_sho_roll", "l_el"])
syncwrite_msg.value.extend([torque_value] * 3)
sync_write_pub.publish(syncwrite_msg)
class ControlModule:
None = 0
DirectControlModule = 1
Framework = 2
if __name__ == '__main__':
rospy.init_node('read_write', anonymous=True)
init_pose_pub = rospy.Publisher('/humanoid_robot_intelligence_control_system/base/ini_pose', String, queue_size=10)
sync_write_pub = rospy.Publisher('/humanoid_robot_intelligence_control_system/sync_write_item', SyncWriteItem, queue_size=10)
dxl_torque_pub = rospy.Publisher('/humanoid_robot_intelligence_control_system/dxl_torque', String, queue_size=10)
write_joint_pub = rospy.Publisher('/humanoid_robot_intelligence_control_system/set_joint_states', JointState, queue_size=10)
write_joint_pub2 = rospy.Publisher('/humanoid_robot_intelligence_control_system/direct_control/set_joint_states', JointState, queue_size=10)
read_joint_sub = rospy.Subscriber('/humanoid_robot_intelligence_control_system/present_joint_states', JointState, joint_states_callback)
button_sub = rospy.Subscriber('/humanoid_robot_intelligence_control_system/open_cr/button', String, button_handler_callback)
control_module = ControlModule.None
demo_ready = False
SPIN_RATE = 30
DEBUG_PRINT = False
loop_rate = rospy.Rate(SPIN_RATE)
manager_name = "/humanoid_robot_intelligence_control_system_manager"
while not rospy.is_shutdown():
rospy.sleep(1.0)
if check_manager_running(manager_name):
rospy.loginfo("Succeed to connect")
break
rospy.logwarn("Waiting for humanoid_robot_intelligence_control_system manager")
ready_to_demo()
while not rospy.is_shutdown():
rospy.spin()
loop_rate.sleep()