def srvSetNineAxisUpdateFrequence(self, params):
print(params)
if params.value not in [15, 30, 60, 100]:
return SetInt32Response()
else:
self.nine_axis_update_frequence = params.value
return SetInt32Response(0, params.value)
def srvCameraSetEnable(self, params):
if params.value == 1 and self.camera.active == False:
ret = self.camera.open_camera(params.port)
return SetInt32Response(1, ret)
elif params.value == 0:
self.camera.active = False
return SetInt32Response(1, 0)
return SetInt32Response()
def srvMotorSetCurrentPosition(self, params):
if params.port == 0:
for i in range(5):
self.d51_driver.motor_set_position(i + 1, params.value)
else:
self.d51_driver.motor_set_position(params.port, params.value)
return SetInt32Response(params.port, params.value)
def srvMotorSetRotate(self, params):
# print(params)
cur_position = Lepi.motor_get_current_position(params.port)
# print(cur_position,params.value*2+cur_position)
Lepi.motor_set_target_position(params.port,
params.value * 2 + cur_position)
return SetInt32Response(params.port, params.value)
def cbLaunchTerminal(self,params):
print(params)
if params.value == 1:
if self.terminal and self.terminal.isalive():
terminated=self.terminal.terminate(force=True)
rospy.loginfo(terminated)
self.terminal = pexpect.spawn("konsole")
else:
if self.terminal and self.terminal.isalive():
terminated=self.terminal.terminate(force=True)
rospy.loginfo(terminated)
return SetInt32Response()
def srvMotorSetPosition(self, params):
self.d51_driver.motor_set_point(params.port, params.value)
return SetInt32Response(params.port, params.value)
def srvMotorSetSpeed(self, params):
self.d51_driver.motor_set_speed(params.port, params.value)
return SetInt32Response(params.port, params.value)
def srvMotorSetRotate(self, params):
position = self.d51_driver.motor_get_position(params.port)
self.d51_driver.motor_set_point(params.port,
params.value * 2 + position)
return SetInt32Response(params.port, params.value)
def srvMotorSetEnable(self,msg): print(msg) Lepi.motor_set_enable(msg.port,msg.value) return SetInt32Response(msg.port,msg.value)
def srvMotorSetType(self, params):
self.d51_driver.motor_set_type(params.port, params.value)
if params.value == 1:
self.d51_driver._motor_set_pulse(params.port, params.value)
return SetInt32Response(params.port, params.value)
def srvSensor9AxesSetEnable(self, params):
# print(params)
if params.port == 0x46:
self.i2c_driver.nineAxisSetEnable(params.value)
return SetInt32Response(params.port, params.value)
return SetInt32Response(params.port, params.value)
def srvCameraSetRectify(self, params):
if params.value == 1:
self.camera.setRectify(True)
elif params.value == 0:
self.camera.setRectify(False)
return SetInt32Response(1, params.value)
def srvMotorSetSpeed(self, params):
# print(params)
Lepi.motor_set_speed(params.port, params.value)
return SetInt32Response(params.port, params.value)
def srvSetResize(self, params):
if params.port <= 480 and params.port >= 10:
if params.value <= 360 and params.value >= 10:
self.detector.setResize(params.port, params.value)
return SetInt32Response(params.port, params.value)
def srvMotorSetType(self, params):
# print(params)
res = Lepi.motor_set_type(params.port, params.value)
# print(res)
return SetInt32Response(params.port, params.value)
def srvMotorSetEnable(self, params):
# print(params)
Lepi.motor_set_enable(params.port, params.value)
return SetInt32Response(params.port, params.value)
def cbSetThreshold(self, params):
self.detector.set_threshold(params.value)
return SetInt32Response(params.port, params.value)
def srvMotorSetPosition(self,msg): print(msg) Lepi.motor_set_target_position(msg.port,msg.value) return SetInt32Response(msg.port,msg.value)
def srvMotorSetSpeed(self,msg): print(msg) Lepi.motor_set_speed(msg.port,msg.value) return SetInt32Response(msg.port,msg.value)
def srvCameraSetFlip(self, params):
self.camera.setFlip(params.value)
return SetInt32Response(0, params.value)
def srvMotorSetCurrentPosition(self, params):
# print(params)
Lepi.motor_set_current_position(params.port, params.value)
return SetInt32Response(params.port, params.value)
def srvSensorSetMode(self, params):
self.d51_driver.sensor_set_mode(params.port, params.value)
return SetInt32Response(params.port, params.value)
def srvSensorSetValue(self, params):
data = Lepi.sensor_set_value(6 - params.port, params.value)
return SetInt32Response(params.port, params.value)
def srvSystemPoweroff(self, params):
self.d51_driver.system_poweroff()
os.system('bash -c "sleep 2 && sudo halt" &')
return SetInt32Response()
def srvSensorSetMode(self, params):
status = Lepi.sensor_set_mode(6 - params.port, params.value)
return SetInt32Response(params.port, params.value)
def cbSetThreshold(self, params):
if params.value > 0 and params.value < 100:
self.detector.setThreshold(params.value)
return SetInt32Response(params.port, params.value)
def srvSetSize(self, params):
if params.port <= 480 and params.port >= 10:
self.IMAGE_W = params.port
if params.value <= 360 and params.value >= 10:
self.IMAGE_H = params.value
return SetInt32Response(params.port, params.value)