diff --git a/humanoid_robot_intelligence_control_system_read_write_demo/CMakeLists.txt b/humanoid_robot_intelligence_control_system_read_write_demo/CMakeLists.txt index 5daccda..582d2fa 100644 --- a/humanoid_robot_intelligence_control_system_read_write_demo/CMakeLists.txt +++ b/humanoid_robot_intelligence_control_system_read_write_demo/CMakeLists.txt @@ -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() diff --git a/humanoid_robot_intelligence_control_system_read_write_demo/package.xml b/humanoid_robot_intelligence_control_system_read_write_demo/package.xml index 3bcd921..77cae1d 100644 --- a/humanoid_robot_intelligence_control_system_read_write_demo/package.xml +++ b/humanoid_robot_intelligence_control_system_read_write_demo/package.xml @@ -28,6 +28,7 @@ the License. catkin roscpp + rospy std_msgs sensor_msgs @@ -35,6 +36,7 @@ the License. humanoid_robot_intelligence_control_system_manager roscpp + rospy std_msgs sensor_msgs @@ -43,6 +45,7 @@ the License. humanoid_robot_intelligence_control_system_manager roscpp + rospy std_msgs sensor_msgs @@ -52,14 +55,16 @@ the License. ament_cmake - roscpp + rclcpp + rclpy std_msgs sensor_msgs humanoid_robot_intelligence_control_system_controller_msgs humanoid_robot_intelligence_control_system_manager - roscpp + rclcpp + rclpy std_msgs sensor_msgs @@ -67,7 +72,8 @@ the License. humanoid_robot_intelligence_control_system_manager - roscpp + rclcpp + rclpy std_msgs sensor_msgs diff --git a/humanoid_robot_intelligence_control_system_read_write_demo/setup.py b/humanoid_robot_intelligence_control_system_read_write_demo/setup.py new file mode 100644 index 0000000..723c5aa --- /dev/null +++ b/humanoid_robot_intelligence_control_system_read_write_demo/setup.py @@ -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) diff --git a/humanoid_robot_intelligence_control_system_read_write_demo/src/read_write.cpp b/humanoid_robot_intelligence_control_system_read_write_demo/src/read_write.cpp deleted file mode 100644 index 949af70..0000000 --- a/humanoid_robot_intelligence_control_system_read_write_demo/src/read_write.cpp +++ /dev/null @@ -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 -#include -#include - -#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("/humanoid_robot_intelligence_control_system/base/ini_pose", 0); - sync_write_pub = nh.advertise( - "/humanoid_robot_intelligence_control_system/sync_write_item", 0); - dxl_torque_pub = nh.advertise("/humanoid_robot_intelligence_control_system/dxl_torque", 0); - write_joint_pub = - nh.advertise("/humanoid_robot_intelligence_control_system/set_joint_states", 0); - write_joint_pub2 = nh.advertise( - "/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/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 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); -} diff --git a/humanoid_robot_intelligence_control_system_read_write_demo/src/read_write.py b/humanoid_robot_intelligence_control_system_read_write_demo/src/read_write.py new file mode 100644 index 0000000..808805f --- /dev/null +++ b/humanoid_robot_intelligence_control_system_read_write_demo/src/read_write.py @@ -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()