mirror of
https://github.com/elephantrobotics/mycobot_ros.git
synced 2026-07-05 19:47:04 +00:00
169 lines
3.8 KiB
Python
Executable file
169 lines
3.8 KiB
Python
Executable file
#!/usr/bin/env python3
|
|
# -*- coding: utf-8 -*-
|
|
import time
|
|
import rospy
|
|
from ultraarm_communication.srv import *
|
|
import pymycobot
|
|
from packaging import version
|
|
# min low version require
|
|
MAX_REQUIRE_VERSION = '3.5.3'
|
|
current_verison = pymycobot.__version__
|
|
print('current pymycobot library version: {}'.format(current_verison))
|
|
if version.parse(current_verison) > version.parse(MAX_REQUIRE_VERSION):
|
|
from pymycobot.ultraArmP340 import ultraArmP340
|
|
class_name = 'new'
|
|
else:
|
|
from pymycobot.ultraArm import ultraArm
|
|
class_name = 'old'
|
|
print("Note: This class is no longer maintained since v3.6.0, please refer to the project documentation: https://github.com/elephantrobotics/pymycobot/blob/main/README.md")
|
|
|
|
|
|
ua = None
|
|
|
|
|
|
def create_handle():
|
|
global ua
|
|
rospy.init_node("ultraArm_services")
|
|
rospy.loginfo("start ...")
|
|
port = rospy.get_param("~port")
|
|
baud = rospy.get_param("~baud")
|
|
rospy.loginfo("%s,%s" % (port, baud))
|
|
if class_name == 'old':
|
|
ua = ultraArm(port, baud)
|
|
else:
|
|
ua = ultraArmP340(port, baud)
|
|
|
|
# Power on the robotic arm
|
|
ua.power_on()
|
|
# calibrate the zero position of the robot arm
|
|
ua.go_zero()
|
|
# time.sleep(3)
|
|
|
|
|
|
def create_services():
|
|
rospy.Service("set_joint_angles", SetAngles, set_angles)
|
|
rospy.Service("get_joint_angles", GetAngles, get_angles)
|
|
rospy.Service("set_joint_coords", SetCoords, set_coords)
|
|
rospy.Service("get_joint_coords", GetCoords, get_coords)
|
|
rospy.Service("switch_gripper_status", GripperStatus, switch_status)
|
|
rospy.Service("switch_pump_status", PumpStatus, toggle_pump)
|
|
rospy.loginfo("ready")
|
|
rospy.spin()
|
|
|
|
|
|
def set_angles(req):
|
|
"""set angles"""
|
|
angles = [
|
|
req.joint_1,
|
|
req.joint_2,
|
|
req.joint_3,
|
|
]
|
|
sp = req.speed
|
|
if ua:
|
|
ua.set_angles(angles, sp)
|
|
|
|
return SetAnglesResponse(True)
|
|
|
|
|
|
def get_angles(req):
|
|
count = 0
|
|
while count < 10:
|
|
if ua:
|
|
angles = ua.get_angles_info()
|
|
if angles != None:
|
|
return GetAnglesResponse(*angles)
|
|
count += 1
|
|
continue
|
|
else:
|
|
return GetAnglesResponse(0.0, 0.0, 0.0)
|
|
|
|
|
|
def set_coords(req):
|
|
coords = [
|
|
req.x,
|
|
req.y,
|
|
req.z,
|
|
]
|
|
sp = req.speed
|
|
if ua:
|
|
ua.set_coords(coords, sp)
|
|
|
|
return SetCoordsResponse(True)
|
|
|
|
|
|
def get_coords(req):
|
|
count = 0
|
|
while count < 10:
|
|
if ua:
|
|
coords = ua.get_coords_info()
|
|
# coords = [176.0, 0.0, 120.0]
|
|
if coords != None:
|
|
return GetCoordsResponse(*coords)
|
|
count += 1
|
|
continue
|
|
else:
|
|
return GetCoordsResponse(176.0, 0.0, 120.0)
|
|
|
|
|
|
def switch_status(req):
|
|
"""Gripper switch,夹爪开关"""
|
|
if ua:
|
|
if req.Status:
|
|
ua.set_gripper_state(0)
|
|
else:
|
|
ua.set_gripper_state(1)
|
|
|
|
return GripperStatusResponse(True)
|
|
|
|
|
|
def toggle_pump(req):
|
|
if ua:
|
|
if req.Status:
|
|
ua.set_gpio_state(1)
|
|
else:
|
|
ua.set_gpio_state(0)
|
|
|
|
return PumpStatusResponse(True)
|
|
|
|
|
|
robot_msg = """
|
|
ultraArm Status
|
|
--------------------------------
|
|
Joint Limit:
|
|
joint 1: -170 ~ +170
|
|
joint 2: 0 ~ 90
|
|
joint 3: 0 ~ 75
|
|
|
|
Coords Limit:
|
|
x: 0 ~ 270
|
|
y: 0 ~ 270
|
|
z: 0 ~ 125
|
|
|
|
Connect Status: %s
|
|
|
|
Servo Infomation: %s
|
|
|
|
Servo Temperature: %s
|
|
"""
|
|
|
|
|
|
def output_robot_message():
|
|
connect_status = False
|
|
servo_infomation = "unknown"
|
|
servo_temperature = "unknown"
|
|
|
|
if ua:
|
|
connect_status = True
|
|
servo_infomation = "all connected"
|
|
|
|
print(
|
|
robot_msg % (connect_status, servo_infomation,
|
|
servo_temperature)
|
|
)
|
|
|
|
|
|
if __name__ == "__main__":
|
|
# print(MyCobot.__dict__)
|
|
create_handle()
|
|
output_robot_message()
|
|
create_services()
|