问题描述
您好，软件开发人员。 我是编程和ROS的新手。我现在正在使用伺服电动机，并且希望使控制节点将电动机旋转到从0-> 90-> 180-> 0度的某个角度。 有一个主代码，我想在主节点中添加def函数：
#!/usr/bin/env python
import os
import rospy
from std_msgs.msg import String
from controller_motor.msg import Motor
from controller_motor.srv import Motor2
if os.name == 'nt':
import msvcrt
def getch():
return msvcrt.getch().decode()
else:
import sys,tty,termios
fd = sys.stdin.fileno()
old_settings = termios.tcgetattr(fd)
def getch():
try:
tty.setraw(sys.stdin.fileno())
ch = sys.stdin.read(1)
finally:
termios.tcsetattr(fd,termios.TCSADRAIN,old_settings)
return ch
from dynamixel_sdk import * # Uses Dynamixel SDK library
class DynamixelController(object):
def __init__(self):
#print("1")
self.motor_sub = rospy.Subscriber("/motor_topic",Motor,self.motor_callback)
self.test_motor_client(1) #here we can put request order as 1 for the using id=14 and position= 1500; or as 2 is id same and position is 2500. The cases Could be found in the server.py node
def motor_callback(self,data):
_id = data.id
_position = data.position
print("_id : {0},_position : {1}".format(_id,_position))
self.read_write(_id,_position)
def test_motor_client(self,request):
print("2")
rospy.wait_for_service('test_motor_service')
try:
proxy = rospy.ServiceProxy('test_motor_service',Motor2)
response = proxy(request)
print("response is id : {} position : {}".format(response.id,response.position))
self.read_write(response.id,response.position)
except rospy.ServiceException as e:
print("Service call Failed: %s"%e)
def new_rotation(self,response):
print("3")
rospy.wait_for_service('test_mototr_rotation')
try:
proxy = rospy.ServiceProxy('test_motor_rotation',Motor2)
response = proxy(request)
self.read_write(response.id,response.position(arg[0]))
sleep(2)
self.read_write(response.id,response._position(arg[1]))
sleep(2)
self.read_write(response.id,response._position(arg[2]))
sleep(2)
except rospy.ServiceException as e:
print("Service call Failed: %s"%e)
def read_write(self,_id,_position):
print("Start the node")
# Control table address
ADDR_PRO_TORQUE_ENABLE = 64 # Control table address is different in Dynamixel model
ADDR_PRO_GOAL_POSITION = 116
ADDR_PRO_PRESENT_POSITION = 132
# Protocol version
PROTOCOL_VERSION = 2.0 # See which protocol version is used in the Dynamixel
# Default setting
DXL_ID = _id # Dynamixel ID : 14
BAUdratE = 1000000 # Dynamixel default baudrate : 1000000
DEVICENAME = '/dev/ttyUSB0' # Check which port is being used on your controller
# ex) Windows: "COM1" Linux: "/dev/ttyUSB0" Mac: "/dev/tty.usbserial-*"
TORQUE_ENABLE = 1 # Value for enabling the torque
TORQUE_disABLE = 0 # Value for disabling the torque
DXL_MINIMUM_POSITION_VALUE = 10 # Dynamixel will rotate between this value
DXL_MAXIMUM_POSITION_VALUE = _position # The position here is already in the degree measurement; and this value (note that the Dynamixel would not move when the position value is out of movable range. Check e-manual about the range of the Dynamixel you use.)
DXL_MOVING_STATUS_THRESHOLD = 20 # Dynamixel moving status threshold
index = 0
dxl_goal_position = [DXL_MINIMUM_POSITION_VALUE,DXL_MAXIMUM_POSITION_VALUE] # Goal position
# Initialize PortHandler instance
# Set the port path
# Get methods and members of PortHandlerLinux or PortHandlerWindows
portHandler = PortHandler(DEVICENAME)
# Initialize PacketHandler instance
# Set the protocol version
# Get methods and members of Protocol1PacketHandler or Protocol2PacketHandler
packetHandler = PacketHandler(PROTOCOL_VERSION)
# Open port
if portHandler.openPort():
print("Succeeded to open the port")
else:
print("Failed to open the port")
print("Press any key to terminate...")
getch()
quit()
# Set port baudrate
if portHandler.setBaudrate(BAUdratE):
print("Succeeded to change the baudrate")
else:
print("Failed to change the baudrate")
print("Press any key to terminate...")
getch()
quit()
# Enable Dynamixel Torque
dxl_comm_result,dxl_error = packetHandler.write1ByteTxRx(portHandler,DXL_ID,ADDR_PRO_TORQUE_ENABLE,TORQUE_ENABLE)
if dxl_comm_result != COMM_SUCCESS:
print("%s" % packetHandler.getTxRxResult(dxl_comm_result))
elif dxl_error != 0:
print("%s" % packetHandler.getRxPacketError(dxl_error))
else:
print("Dynamixel has been successfully connected")
while 1:
print("Press any key to continue! (or press ESC to quit!)")
if getch() == chr(0x1b):
break
# Write goal position
dxl_comm_result,dxl_error = packetHandler.write4ByteTxRx(portHandler,ADDR_PRO_GOAL_POSITION,dxl_goal_position[index])
if dxl_comm_result != COMM_SUCCESS:
print("%s" % packetHandler.getTxRxResult(dxl_comm_result))
elif dxl_error != 0:
print("%s" % packetHandler.getRxPacketError(dxl_error))
while 1:
# Read present position
dxl_present_position,dxl_comm_result,dxl_error = packetHandler.read4ByteTxRx(portHandler,ADDR_PRO_PRESENT_POSITION)
if dxl_comm_result != COMM_SUCCESS:
print("%s" % packetHandler.getTxRxResult(dxl_comm_result))
elif dxl_error != 0:
print("%s" % packetHandler.getRxPacketError(dxl_error))
print("[ID:%03d] GoalPos:%03d PresPos:%03d" % (DXL_ID,dxl_goal_position[index],dxl_present_position))
if not abs(dxl_goal_position[index] - dxl_present_position) > DXL_MOVING_STATUS_THRESHOLD:
break
# Change goal position
if index == 0:
index = 1
else:
index = 0
# disable Dynamixel Torque
dxl_comm_result,TORQUE_disABLE)
if dxl_comm_result != COMM_SUCCESS:
print("%s" % packetHandler.getTxRxResult(dxl_comm_result))
elif dxl_error != 0:
print("%s" % packetHandler.getRxPacketError(dxl_error))
# Close port
portHandler.closePort()
if __name__ == '__main__':
rospy.init_node('read_write',anonymous=True)
controller = DynamixelController()
这是服务器节点：
#!/usr/bin/env python
import rospy
from controller_motor.srv import Motor2,Motor2Response
def motor_info(req):
print("receive the request : {}".format(req.order))
if req.order == 1:
_id = 14
#_position = 1500
_positiondegree = 180 # change it degree value
_position = (11.4 * (_positiondegree)) # here is using there 11.4 the number which helps rotate the motor in the correct degree; We get the number encoder number/degree we want to rotate the motor.
elif req.order == 2:
_id = 14
#_position = 2047.5
_positiondegree = 180
_position = (11.4 * (_positiondegree))
print("Response to this request id : {} and postiion : {}".format(_id,_position))
return Motor2Response(_id,_position)
def motor_info_server():
rospy.init_node('test_motor_service')
s = rospy.Service('test_motor_service',Motor2,motor_info)
print("Ready to response some id and position")
# spin() simply keeps python from exiting until this node is stopped
rospy.spin()
if __name__ == '__main__':
motor_info_server()
现在，在运行电动机时，它会旋转到我希望旋转的程度。现在我想知道如何给电机一些角度，以使电动机从0旋转到其他角度，然后再次回到0度位置。
我猜可能在一定程度上应用了列表和参数。但是我不知道该怎么做:) 我想从像您这样的专业人士那里获得一些建议，伙计：）
我知道我的问题对于专业人士可能是显而易见的。我想尽可能地获得编程和ROS的经验：）
我将非常感谢您的帮助，并建议我该如何做：）
感谢所有与他分享想法的人：）
我希望你有个美好的一天：）
解决方法
您必须在客户端文件中的代码中添加新功能：
def rotate_bot(self,degree):
rospy.wait_for_service('rotate_motor')
try:
proxy = rospy.ServiceProxy('test_motor_rotation',Motor2)
response = proxy(degree)
except rospy.ServiceException as e:
print("Service call failed: %s"%e)
此功能基本上称为服务，并向他提供机器人必须旋转的度数。还要添加此函数以调用roate函数：
def test_rotate(self):
rotate_bot(90)
rotate_bot(90)
rotate_bot(-180)
现在机器人从0转到> 90-> 180转到0
在服务器文件中，您必须使用motor_info_server()方法添加服务：
s = rospy.Service('test_motor_rotation',Motor2,rotate_motor)
并添加方法：
def rotate_motor(req):
print("received the request : {}".format(req.order))
degree = req.order
_id = 14
_positiondegree = degree
_position = (11.4 * (_positiondegree))
return Motor2Response(_id,_position)
如果您希望通过双精度学位，则必须将消息数据类型从Motor2更改为其他类型...