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.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()