121 lines
5.0 KiB
Python
Executable File
121 lines
5.0 KiB
Python
Executable File
#!/usr/bin/env python3
|
|
|
|
# This Script is a simple demonstration of controlling SmarGonMCS2
|
|
# via the RESTful API provided by the smargopolo server
|
|
#
|
|
# Basic functions are demonstrated like:
|
|
# 1) See if the server is running, and if SmarGon is ready to move.
|
|
# 2) Get current position in SCS (SmarGon Coordinate System)
|
|
# 3) Do a move to an absolute position
|
|
# 4) Do a relative move
|
|
#
|
|
# Wayne Glettig, 17.02.2021
|
|
|
|
import requests
|
|
import json
|
|
import time
|
|
from sys import exit
|
|
|
|
|
|
# Set the smargopolo server address and port here:
|
|
smargopolo_server = "http://smargopolo:3000"
|
|
|
|
##########################################################################################################
|
|
# 1) See if the server is running and if SmarGon is ready to move:
|
|
print("Getting HTTP GET request from "+smargopolo_server+"/readbackMCS")
|
|
response = requests.get(smargopolo_server+"/readbackMCS")
|
|
|
|
print("Ok, the server gives us the following response:")
|
|
json_txt = json.dumps(response.json(),indent = 4) # Pretty Print the JSON response
|
|
print (json_txt)
|
|
|
|
#Convert the JSON response into a python object for further use
|
|
readbackMCS = json.loads(response.text)
|
|
|
|
#We can print the serial number of the SmarAct MCS2 Controller
|
|
print ('SmarAct MCS2 S/N: "'+ readbackMCS['serial_number'] +'"')
|
|
|
|
#We can print the last message was logged on ROS (/rosout)
|
|
print ('Last rosout Message: "'+ readbackMCS['rosout']['msg'] +'"')
|
|
|
|
#We can check if the loop counter is counting, so if the internal smargopolo feedback loop
|
|
#in ROS is running correctly
|
|
loopcounter1 = readbackMCS['seq']
|
|
time.sleep(1)
|
|
response = requests.get(smargopolo_server+"/readbackMCS")
|
|
readbackMCS = json.loads(response.text)
|
|
loopcounter2 = readbackMCS['seq']
|
|
if (loopcounter2 > loopcounter1):
|
|
print('Loopcounter counting correctly. Good.')
|
|
else:
|
|
print('Loopcounter not counting. Not good. ROS control loop has an issue. Stopping script.')
|
|
exit(0)
|
|
|
|
# A good way to see if the smargon is ready to be used is to check if it is in "MODE 2: READY (Follow Target)"
|
|
print("Let's see in which MODE the smargopolo controller is:")
|
|
readbackMCS = json.loads(response.text) #Convert the JSON response into a python object
|
|
MODE = readbackMCS['mode']
|
|
if (MODE == 0):
|
|
print ("MODE 0: UNINITIALZED -> Should we initialize? (y/n)")
|
|
if (input()=="y"):
|
|
print ("Running initialization (Setting MODE 1: INITIALIZING...)")
|
|
response = requests.put(smargopolo_server+"/mode?mode=1")
|
|
while (MODE != 2): #keep polling the server until MODE = 2
|
|
time.sleep(1)
|
|
response = requests.get(smargopolo_server+"/readbackMCS")
|
|
readbackMCS = json.loads(response.text) #Convert the JSON response into a python object
|
|
MODE = readbackMCS['mode']
|
|
else:
|
|
exit(0)
|
|
if (MODE == 2):
|
|
print ("MODE 2: READY (Follow Target) -> ready to roll!")
|
|
|
|
##########################################################################################################
|
|
# 2) Get current position in SCS (SmarGon Coordinate System)
|
|
print ("Get Current position in SCS (SmarGon Coordinate System):")
|
|
response = requests.get(smargopolo_server+"/readbackSCS")
|
|
print (json.dumps(response.json(),indent = 4)) # Pretty Print the JSON response
|
|
|
|
# Again here, the JSON response can be converted into a python object for further use
|
|
readbackSCS = json.loads(response.text)
|
|
SHZ = readbackSCS['SHZ']
|
|
print("The readback SHZ value is:" , SHZ)
|
|
|
|
##########################################################################################################
|
|
# 3) Do a move to an absolute position
|
|
print("Moving to Home Position")
|
|
response = requests.put(smargopolo_server+'/targetSCS?SHX=0&SHY=0&SHZ=18&CHI=0&PHI=0&OX=0&OY=0&OZ=180')
|
|
|
|
print (json.dumps(response.json(),indent = 4)) # optionally, pretty-print the JSON response
|
|
# Note that the JSON response is the confirmed target position, not the readback position.
|
|
# If you need a confirmation that SmarGon actually moved to a certain position,
|
|
# You will have to request the current readback position and check that.
|
|
|
|
time.sleep(2)
|
|
|
|
##########################################################################################################
|
|
# 4) Do a few relative moves
|
|
print("Doing relative moves")
|
|
response = requests.put(smargopolo_server+'/targetSCS_rel?SHX=0.1')
|
|
time.sleep(0.5)
|
|
response = requests.put(smargopolo_server+'/targetSCS_rel?SHY=0.1')
|
|
time.sleep(0.5)
|
|
response = requests.put(smargopolo_server+'/targetSCS_rel?SHX=-0.2')
|
|
time.sleep(0.5)
|
|
response = requests.put(smargopolo_server+'/targetSCS_rel?SHY=-0.2')
|
|
time.sleep(0.5)
|
|
response = requests.put(smargopolo_server+'/targetSCS_rel?SHX=0.2')
|
|
time.sleep(0.5)
|
|
response = requests.put(smargopolo_server+'/targetSCS_rel?SHY=0.1')
|
|
time.sleep(0.5)
|
|
response = requests.put(smargopolo_server+'/targetSCS_rel?SHX=-0.1')
|
|
time.sleep(0.5)
|
|
|
|
##########################################################################################################
|
|
time.sleep(1)
|
|
print("And now back home")
|
|
response = requests.put(smargopolo_server+'/targetSCS?SHX=0&SHY=0&SHZ=18&CHI=0&PHI=0&OX=0&OY=0&OZ=180')
|
|
time.sleep(2)
|
|
|
|
print("Ok, All done.")
|