def set_logging_callback(self, req):
if req.data and not self.start_logging:
self.start_logging = True
# Get params
self.get_params()
# Subscribe to robot data
self.subscribe_robot_data()
# Create POST timers
self.create_post_timers()
elif not req.data and self.start_logging:
# Close open segment
segment_closed, message = self.close_current_segment()
if segment_closed:
self.start_logging = False
# Unsubscribe to robot data
self.unsubscribe_robot_data()
# Stop POST timers
self.stop_post_timers()
else:
return SetBoolResponse(False, message)
else:
if req.data:
rospy.logwarn("Tried to start logging, but it's already running.")
else:
rospy.logwarn("Tried to stop logging, but it's not running.")
return SetBoolResponse(True, "Logging is " + ("started." if req.data else "stopped."))
def callback_state(self, start_stop):
"""
State callback used to start and stop the robot.
:param start_stop: start stop command.
:type start_stop: SetBool
"""
# If the robot should start and in not active
if start_stop.data == True and not self.active:
self.active = True
rospy.logdebug('[Bug 2] Starting')
if self.state == Bug2State.GO_TO_POINT:
rospy.logdebug('[Bug 2] Starting Go To Point')
resp = self.start_go_to_point(True)
resp = self.start_wall_follower(False)
# If the robot should stop and is active.
elif start_stop.data == False and self.active:
self.active = False
rospy.logdebug('[Bug 2] Stopping')
resp = self.start_go_to_point(False)
resp = self.start_wall_follower(False)
return SetBoolResponse(True, 'Stopping Bug 2')
else:
if start_stop.data == True:
return SetBoolResponse(False, 'Already Doing Bug 2')
else:
return SetBoolResponse(False, 'Already Stopped')
def callback_follow_left(self, follow_left):
if follow_left.data == True:
self.follow_side = FollowSide.LEFT
return SetBoolResponse(True, 'Following Left')
else:
self.follow_side = FollowSide.RIGHT
return SetBoolResponse(True, 'Following Right')
def handle_record(msg):
if msg.data:
# Start recording the video
try:
recording(True)
return SetBoolResponse(True, '')
except:
return SetBoolResponse(False, 'Unable to start recording')
else:
# Stop recording and publish thumbnail & video files
try:
recording(False)
except Exception as e:
return SetBoolResponse(
False, 'Unable to stop recording: {0}'.format(e))
thumbhash = ''
try:
m = list(medias())[-1]
msg = String()
thumbhash = ipfsPublish(getThumb(m))
msg.data = thumbhash
thumbnail.publish(msg)
videohash = ipfsPublish(getVideo(m))
msg.data = videohash
video.publish(msg)
except Exception as e:
return SetBoolResponse(
False, 'Unable to publish media: {0}'.format(e))
return SetBoolResponse(True, thumbhash)
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 callback_isWater(self, is_water):
if is_water.data == True:
self.isWater = True
return SetBoolResponse(True, 'Mapping water')
elif is_water.data == False:
self.isWater = False
return SetBoolResponse(True, 'No longer mapping water')
def enable_control_srv(self, msg):
if msg.data:
self.enable_control = True
return SetBoolResponse(True, "Turned on " + self.model_name + " Controller")
else:
self.enable_control = False
return SetBoolResponse(True, "Turned off " + self.model_name + " Controller" )
def handle_enable_detector(self, req):
"""
Service callback function for enabling object detector
"""
if isinstance(req, SetBoolRequest):
self.detector_enabled = req.data
return SetBoolResponse(True, '')
return SetBoolResponse(False, 'Wrong data type')
def handle_set_debug_image(self, req):
"""
Service callback function for setting debug_image variable
"""
if isinstance(req, SetBoolRequest):
self.debug_image = req.data
return SetBoolResponse(True, '')
return SetBoolResponse(False, 'Wrong data type')
def toggle_handler(self, request):
rospy.loginfo("Requesting to set running_flag to {0:}".format(request.data))
self.running_flag = request.data
if request.data:
resp = SetBoolResponse(True, "Running")
else:
resp = SetBoolResponse(True, "Stopped")
return resp
def handle_stair_mode(self, req):
"""ROS service handler to set a stair mode to the robot."""
try:
mobility_params = self.spot_wrapper.get_mobility_params()
mobility_params.stair_hint = req.data
self.spot_wrapper.set_mobility_params( mobility_params )
return SetBoolResponse(True, 'Success')
except Exception as e:
return SetBoolResponse(False, 'Error:{}'.format(e))
def stringCallback(req):
# 显示请求数据
if req.data:
rospy.loginfo("Hello ROS!")
# 反馈数据
return SetBoolResponse(True, "Print Successully")
else:
# 反馈数据
return SetBoolResponse(False, "Print Failed")
def activacion(self, req):
if req.data == True:
self.flag = True
rospy.loginfo(rospy.get_caller_id() + "Recibida peticion de True")
return SetBoolResponse(True, "Activado")
else:
self.flag = False
rospy.loginfo(rospy.get_caller_id() + "Recibida peticion de False")
return SetBoolResponse(False, "Desactivado")
def moveCloser(self, req):
if req.data:
self.approach_now = True
self.orientation_fixed = False
self.update_robot_pose()
return SetBoolResponse(True, "Service is successfully called")
else:
self.approach_now = False
return SetBoolResponse(False, "Service is not successfully called")
def string_callback(req):
getpoetry()
if req.data:
ttt = rospy.get_param('truth').replace('。', '')
print(ttt)
print(poetry[ttt])
#text = "床前明月光，疑是地上霜。举头望明月，低头思故乡。".encode('gbk')
XF_text(poetry[ttt].encode('gbk'))
return SetBoolResponse(True, "Print Successully")
else:
return SetBoolResponse(False, "Print Failed")
def _lock_cb(self, msg):
"""
@brief Can be used to get sync access to server
"""
if msg.data:
self._mutex.acquire()
else:
try:
self._mutex.release()
except BaseException:
return SetBoolResponse(False, "Mutex already unlocked.")
return SetBoolResponse(True, "Ok")
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 toogle_relay(req):
global pub
msg = PwmCmd()
msg.pin = 6
response = SetBoolResponse(0, 'relay desactivated (open)')
if req.data:
msg.command = 100
response = SetBoolResponse(0, 'relay activated (closed)')
else:
msg.command = -100
pub.publish(msg)
return response
def node_state(message):
if message.data:
username = os.environ.get("USER")
map_path = "/home/" + username + "/maps_temp/map.yaml"
p = Popen([
"rosrun", "map_server", "map_server", "__name:=map_server",
map_path
])
return SetBoolResponse(True, "enable")
else:
if kill_node("map_server"):
return SetBoolResponse(True, "disable")
else:
return SetBoolResponse(False, "disable")
def callback_state(self, start_stop):
if start_stop.data == True and not self.explore:
self.explore = True
return SetBoolResponse(True, 'Starting Exploring')
elif start_stop.data == False and self.explore:
self.explore = False
self.stopped = False
# setting explore to false and stopped to false will cause the next laser scan
# callback to send a stop command then set stopped to true.
return SetBoolResponse(True, 'Stopping Exploring')
else:
if start_stop.data == True:
return SetBoolResponse(False, 'Already Exploring')
else:
return SetBoolResponse(False, 'Already Stopped')
def setOperationalCallback(self, req):
if not self.configured:
return SetBoolResponse(success = False, message = 'Node is not configured.')
# start/stop timer
if req.data:
# timer must be running
if not self.timer:
self.timer = rospy.Timer(rospy.Duration(self.period), self.timerCallback)
else:
# kill timer
if self.timer:
self.timer.shutdown()
self.timer = None
# success
return SetBoolResponse(success = True)
def _enable_controller_service_callback(self, req):
self._controller_enabled = req.data
if self._controller_enabled:
rospy.logwarn("Engine controller set to enabled")
else:
rospy.logwarn("Engine controller set to disabled")
return SetBoolResponse(success=True, message="")
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 srv_cb(self, req):
if req.data:
self._calltimer = rospy.Timer(rospy.Duration(2.0),
self.callsrv,
oneshot=True)
rospy.set_param("/received_service_call", True)
return SetBoolResponse(True, "")
def wall_follower_switch_counterclockwise(req):
global active_
active_ = req.data
res = SetBoolResponse()
res.success = True
res.message = 'Done!'
return res
def my_callback(request):
#change robot listing mode depening on request.data (true or false)
print "Callback switch_speechmode has been called"
return SetBoolResponse(
success= True,
message="switched robotlistening to " + str(request.data)
)
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 _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 _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 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
