def handle_close_gripper(self, req):
try:
if req.data == True:
# we close
self.stop_gripper()
self.gripper_state = 'Close'
self.close_gripper()
rospy.sleep(0.5)
elif req.data == False:
self.stop_gripper()
self.gripper_state = 'Open'
self.open_gripper()
rospy.sleep(0.5)
while self.is_moving: # wait until the gripper stops
rospy.sleep(0.1)
resp = SetBoolResponse()
resp.success = True
rospy.loginfo('Done')
return resp
except rospy.ServiceException as exc:
rospy.logerr("Service did not process request: " + str(exc))
resp = SetBoolResponse()
resp.success = False
return resp
def _allow_network(self, req):
data = req.data
resp = SetBoolResponse()
resp.success = False
if pulseaudio.submit_module(
'module-zeroconf-discover',
'load' if data else 'unload',
) != 0:
resp.message = "Couldn't load module zeroconf discover"
return resp
if pulseaudio.submit_module(
'module-native-protocol-tcp',
'load' if data else 'unload',
'auth-anonymous=1',
) != 0:
resp.message = "Couldn't load module native protocol"
return resp
if pulseaudio.submit_module(
'module-zeroconf-publish',
'load' if data else 'unload',
) != 0:
resp.message = "Couldn't load module zeroconf publish"
return resp
resp.success = True
return resp
def enableChargeCb(self, req):
'''
ROS service server to start/stop the charge
'''
response = SetBoolResponse()
if req.data:
# Enable charge if ready to charge
if self.ready_to_charge and ((self.now - self.io_last_stamp) < RECEIVED_IO_TIMEOUT):
res_charge_off = self.write_digital_output_client(self.charge_off_output, False)
res_charge_on = self.write_digital_output_client(self.charge_on_output, req.data)
response.success = res_charge_off.ret and res_charge_on.ret
if response.success:
response.message = 'ok'
else:
response.message = 'error'
self.last_enable_charge_msg_sent = True
else:
rospy.logwarn('%s::enableCharge: The robot is not ready to charge'%self.node_name)
response.success = False
response.message = 'not ready to start charging'
self.last_enable_charge_msg_sent = False
else:
# Disable charge
res_charge_on = self.write_digital_output_client(self.charge_on_output, req.data)
res_charge_off = self.write_digital_output_client(self.charge_off_output, True)
print(res_charge_on, res_charge_off)
response.success = res_charge_on.ret and res_charge_off.ret
if response.success:
response.message = 'ok'
else:
response.message = 'error'
self.last_enable_charge_msg_sent = False
return response
def handleSetEnabledSrv(self, req):
res = SetBoolResponse()
res.success = False
try:
self.listeningMovementProxy.setEnabled(req.data)
res.success = True
return res
except RuntimeError, e:
rospy.logerr("Exception caught:\n%s", e)
return res
def callback_srv(data):
resp = SetBoolResponse()
if data.data == True:
resp.message = "called"
resp.success = True
else:
resp.message = "ready"
resp.success = False
print(resp.message)
return resp
def handleSetEnabledSrv(self, req):
res = SetBoolResponse()
res.success = False
try:
self.backgroundMovementProxy.setEnabled(req.data)
res.success = True
return res
except RuntimeError, e:
rospy.logerr("Exception caught:\n%s", e)
return res
def handleDisableBasicAwareness(self, req):
res = SetBoolResponse()
res.success = False
try:
self.proxy.basic_awareness.setEnabled(False) #True of False
res.success = True
res.message = str("Disabled")
return res
except RuntimeError, e:
rospy.logerr("Exception caught:\n%s", e)
return res
def crash_callback(self, request):
response = SetBoolResponse()
avg_lsr = (self.laser_data.ranges[-2] + self.laser_data.ranges[2]) / 2
if avg_lsr <= self.distance:
# close enough to wall
response.success = True
else:
response.success = False
return response
def handle_service(self, req):
rep = SetBoolResponse()
try:
self.ard.setLed(req.data)
rep.success = True
rep.message = "succeed"
except Exception as e:
rep = SetBoolResponse()
rep.success = False
rep.message = e
return rep
def taskStartCallback(self, req):
res = SetBoolResponse()
if req.data == True:
self.task_start_ = True
res.success = True
res.message = "Task Start"
rospy.loginfo("Task Start")
else:
self.task_start_ = False
res.success = False
res.message = "Task does not start"
rospy.loginfo("Task does not start")
return res
def _loopback(self, req):
data = req.data
resp = SetBoolResponse()
resp.success = False
if pulseaudio.submit_module(
'module-loopback',
'load' if data else 'unload',
) != 0:
resp.message = "Couldn't load module native protocol"
return resp
resp.success = True
return resp
def changeState_handler(req):
global g_state
rospy.loginfo("Service called with data = %d", req.data)
response = SetBoolResponse()
if req.data == g_state:
response.success = False
response.message = "Incorrect call of the service"
else:
g_state = req.data
response.success = True
response.message = "Service succesfully called"
return response
def safetyCallback(req):
global safety_flag
safety_flag = bool(req.data)
safety_resp = SetBoolResponse()
safety_resp.success = True
safety_resp.message = ""
return safety_resp
def go_to_point_switch(req):
global active_
active_ = req.data
res = SetBoolResponse()
res.success = True
res.message = 'Done!'
return res
def save_cb(self, req):
if req.data == True:
self.save_calibrate_result()
res = SetBoolResponse()
res.success = True
res.message = "recieved"
return res
def _movement_controller_state_callback(self, req):
'''
Callback function for the movement controller enable/disable service. If true,
the movement controller will make corrections to meet the desired speed and
steering direction with PID controller. Else if disabled, the controller will
simply send PWM values of [0, 0, 0, 0] to the motor driver.
Parameters:
req: The request message (bool) to enable/disable the motor. Type std_srvs/SetBool.
Returns:
response: A response with a string & bool notifiying if the request was valid. Type std_srvs/SetBoolResponse
'''
self.movement_controller_enabled = req.data
response = SetBoolResponse()
response.success = True
if (self.movement_controller_enabled):
response.message = "Movement Controller Enabled"
else:
response.message = "Movement Controller Disabled"
pwm_msg = Int16MultiArray()
pwm_msg.data = [0, 0, 0, 0]
self.pwm_pub.publish(pwm_msg)
return response
def srvSwitch(self, request):
"""
Args:
request (:obj:`std_srvs.srv.SetBool`): The switch request from the ``~switch`` callback.
Returns:
:obj:`std_srvs.srv.SetBoolResponse`: Response for successful feedback
"""
old_state = self._switch
new_state = request.data
self._switch = new_state
for pub in self.publishers:
pub.active = self._switch
for sub in self.subscribers:
sub.active = self._switch
msg = 'Node switched from %s to %s' % ('on' if old_state else 'off',
'on' if new_state else 'off')
response = SetBoolResponse()
response.success = True
response.message = msg
self.log(msg)
return response
def wall_follower_switch_counterclockwise(req):
global active_
active_ = req.data
res = SetBoolResponse()
res.success = True
res.message = 'Done!'
return res
def start_search(msg):
global min_angle, align_dist, increment, goal, marker_detected, marker_search, marker_aligned, goal_set, goal_published, rot, motion
marker_search = msg.data
if not marker_search:
min_angle = 0.02
increment = min_angle * 10
goal = [0, 0, 0]
marker_detected = False
marker_search = False
marker_aligned = False
goal_set = False
goal_published = False
rot = False
motion = False
align_dist = False
try:
goal_stop(True)
goal_reset()
odom_reset()
goal_stop(False)
except rospy.ServiceException as e:
print("Service call falied: %s" % e)
response = SetBoolResponse()
response.success = True
rospy.loginfo("Searching Set: {}".format(marker_search))
return response
def _srv_switch(self, request):
"""
Args:
request (:obj:`std_srvs.srv.SetBool`): The switch request from the ``~switch`` callback.
Returns:
:obj:`std_srvs.srv.SetBoolResponse`: Response for successful feedback
"""
old_state = self._switch
self._switch = new_state = request.data
# propagate switch change to publishers and subscribers
for pub in self.publishers:
pub.active = self._switch
for sub in self.subscribers:
sub.active = self._switch
# update node switch in the diagnostics manager
if DTROSDiagnostics.enabled():
DTROSDiagnostics.getInstance().update_node(
enabled=self._switch
)
# create a response to the service call
msg = 'Node switched from [%s] to [%s]' % (
'on' if old_state else 'off',
'on' if new_state else 'off'
)
# print out the change in state
self.log(msg)
# reply to the service call
response = SetBoolResponse()
response.success = True
response.message = msg
return response
def start_cb(self, req):
if req.data == True:
self.odom_start = self.odom[:]
self.start = True
res = SetBoolResponse()
res.success = True
res.message = "recieved"
return res
def __saveImg_callback(self, req):
"""Service server request callback."""
res = SetBoolResponse()
if req.data:
result = self.save()
res.success = result
res.message = self.__file_name if result else 'image not ready'
else:
"""Data of requset is false then change name of directory."""
self.__set_saveDirectory()
res.success = True
res.message = self.__directory
print 'Change directory:', self.__directory
self.__save_count = 0
return res
def goal_stop(msg):
global stop
stop = msg.data
response = SetBoolResponse()
response.success = True
response.message = "{} ".format(stop)
rospy.loginfo("Goal Stopped Set: {}".format(stop))
return response
def save_cb(self, req):
if req.data == True:
rospy.loginfo("Save File")
self.close_file()
res = SetBoolResponse()
res.success = True
res.message = "recieved"
return res
def finish_diving_cb(self, req):
if req.data == True:
rospy.loginfo("Finish Diving")
self.finish_diving = True
res = SetBoolResponse()
res.success = True
res.message = "recieved"
return res
def srv_cb(self, req):
self.safe_operation = req.data
ans = SetBoolResponse()
ans.success = True
ans.message = self.event_msg + "{}".format(req.data)
return ans
def emergency_stop_cb(self, req):
if req.data == True:
self.emergency_stop = True
else:
self.emergency_stop = False
res = SetBoolResponse()
res.success = True
res.message = "recieved"
return res
def reset_cb(self, req):
if req.data == True:
self.full_goals = []
self.goals = []
self.get_path = False
res = SetBoolResponse()
res.success = True
res.message = "recieved"
return res
def call_set_bool_common(self, event, client, request):
try:
resp = client.call(request)
wx.CallAfter(self.update_reply_show, resp)
except rospy.ServiceException, e:
resp = SetBoolResponse()
resp.success = False
resp.message = 'no such service in simulation'
wx.CallAfter(self.update_reply_show, resp)
def clear_cb(self, req):
if req.data == True:
self.clear = True
else:
self.clear = False
res = SetBoolResponse()
res.success = True
res.message = "recieved"
return res
def set_safety_tag(self, req):
self.safe_operation = req.data
ans = SetBoolResponse()
ans.success = True
ans.message = "safe operation: {}".format(req.data)
return ans
def start_aruco_place(self, req):
success = self.pick_type.pick_and_place_aruco("place")
reply = SetBoolResponse()
reply.success = success
reply.message = ""
return reply
