latest pushes
This commit is contained in:
parent
e58fc1b9b7
commit
32ac6c1da5
@ -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()
|
||||
|
||||
@ -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">
|
||||
|
||||
@ -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)
|
||||
@ -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);
|
||||
}
|
||||
@ -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()
|
||||
Loading…
Reference in New Issue
Block a user