Commit fc6bc64b authored by Alexis Nicolin's avatar Alexis Nicolin

Added ROS service: stopReadingQueue

A bit more intuitive than sending readingQueue(-1).
parent 8b7232d4
import rospy
from std_srvs.srv import Trigger, TriggerResponse, SetBool, SetBoolResponse
from std_srvs.srv import Trigger, TriggerResponse, SetBool, SetBoolResponse, Empty, EmptyResponse
from sot_hpp_msgs.srv import PlugSot, PlugSotResponse, SetString, SetJointNames, GetInt, GetIntResponse, SetInt, SetIntRequest, SetIntResponse
from dynamic_graph_bridge_msgs.srv import RunCommand
......@@ -11,6 +11,7 @@ class RosInterface(object):
rospy.Service('/sot/request_hpp_topics', Trigger, self.requestHppTopics)
rospy.Service('/sot/clear_queues', Trigger, self.clearQueues)
rospy.Service('/sot/read_queue', SetInt, self.readQueue)
rospy.Service('/sot/stop_reading_queue', Empty, self.stopReadingQueue)
self.runCommand = rospy.ServiceProxy ('/run_command', RunCommand)
self.supervisor = supervisor
......@@ -112,6 +113,16 @@ class RosInterface(object):
print answer
return SetIntResponse ()
def stopReadingQueue(self, req):
if self.supervisor is not None:
self.supervisor.readQueue( -1 )
cmd = "supervisor.readQueue(" + str(-1) + ")"
answer = self.runCommand (cmd)
print cmd
print answer
return EmptyResponse ()
def requestHppTopics(self, req):
handlers = {
'hppcom': rospy.ServiceProxy ('/hpp/target/add_center_of_mass', SetString),
Markdown is supported
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment