100 lines
2.7 KiB
Python
100 lines
2.7 KiB
Python
import roslibpy.actionlib
|
|
|
|
client = roslibpy.Ros(host='10.10.10.10', port=9090)
|
|
client.run()
|
|
|
|
print(f"Is ROS connected? {client.is_connected}")
|
|
|
|
|
|
def request(service_name, type, value=None):
|
|
service = roslibpy.Service(client, service_name, type)
|
|
request = roslibpy.ServiceRequest(value)
|
|
return service.call(request)
|
|
|
|
|
|
request('niryo_robot_tools_commander/update_tool', 'std_srvs/Trigger')
|
|
request('niryo_robot_tools_commander/enable_tcp', "std_srvs/Bool", {'value': True})
|
|
|
|
|
|
action = roslibpy.actionlib.ActionClient(client, '/niryo_robot_arm_commander/robot_action', '/niryo_robot_arm_commander/RobotMoveAction')
|
|
goal = roslibpy.actionlib.Goal(action, roslibpy.Message({'cmd': {
|
|
'cmd_type': 1,
|
|
'position': {
|
|
'x': 0.2,
|
|
'y': 0.2,
|
|
'z': 0.02
|
|
},
|
|
'rpy': {
|
|
'roll': 0,
|
|
'pitch': 1.57,
|
|
'yaw': 0
|
|
}
|
|
}}))
|
|
|
|
goal.send()
|
|
goal.wait(10)
|
|
|
|
|
|
action = roslibpy.actionlib.ActionClient(client, "/niryo_robot_tools_commander/action_server", '/niryo_robot_tools_commander/ToolAction')
|
|
goal = roslibpy.actionlib.Goal(action, roslibpy.Message({
|
|
'cmd': {
|
|
'cmd_type': 2,
|
|
'tool_id': 11,
|
|
'max_torque_percentage': 100,
|
|
'hold_torque_percentage': 30
|
|
}
|
|
}))
|
|
|
|
goal.send()
|
|
goal.wait(10)
|
|
|
|
action = roslibpy.actionlib.ActionClient(client, '/niryo_robot_arm_commander/robot_action', '/niryo_robot_arm_commander/RobotMoveAction')
|
|
goal = roslibpy.actionlib.Goal(action, roslibpy.Message({'cmd': {
|
|
'cmd_type': 1,
|
|
'position': {
|
|
'x': 0.2,
|
|
'y': 0.2,
|
|
'z': 0.2
|
|
},
|
|
'rpy': {
|
|
'roll': 0,
|
|
'pitch': 1.57,
|
|
'yaw': 0
|
|
}
|
|
}}))
|
|
|
|
goal.send()
|
|
goal.wait(10)
|
|
|
|
action = roslibpy.actionlib.ActionClient(client, '/niryo_robot_arm_commander/robot_action', '/niryo_robot_arm_commander/RobotMoveAction')
|
|
goal = roslibpy.actionlib.Goal(action, roslibpy.Message({'cmd': {
|
|
'cmd_type': 1,
|
|
'position': {
|
|
'x': -0.2,
|
|
'y': -0.2,
|
|
'z': 0.02
|
|
},
|
|
'rpy': {
|
|
'roll': 0,
|
|
'pitch': 1.57,
|
|
'yaw': 0
|
|
}
|
|
}}))
|
|
|
|
goal.send()
|
|
goal.wait(10)
|
|
|
|
action = roslibpy.actionlib.ActionClient(client, "/niryo_robot_tools_commander/action_server", '/niryo_robot_tools_commander/ToolAction')
|
|
goal = roslibpy.actionlib.Goal(action, roslibpy.Message({
|
|
'cmd': {
|
|
'cmd_type': 1,
|
|
'tool_id': 11,
|
|
'max_torque_percentage': 100,
|
|
'hold_torque_percentage': 30
|
|
}
|
|
}))
|
|
|
|
goal.send()
|
|
goal.wait(10)
|
|
|