Commit 32c7d83a authored by Joseph Mirabel's avatar Joseph Mirabel Committed by Joseph Mirabel
Browse files

Add Supervisor.publishState

parent 438420df
......@@ -20,6 +20,7 @@ class RosInterface(object):
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)
rospy.Service('/sot/publish_state', Empty, self.publishState)
wait_for_service ("/run_command")
self._runCommand = rospy.ServiceProxy ('/run_command', RunCommand)
self.supervisor = supervisor
......@@ -128,6 +129,14 @@ class RosInterface(object):
answer = self.runCommand (cmd)
return EmptyResponse ()
def publishState(self, req):
if self.supervisor is not None:
self.supervisor.publishState ()
cmd = "supervisor.publishState()"
answer = self.runCommand (cmd)
return EmptyResponse ()
def requestHppTopics(self, req):
for srv in ['add_center_of_mass', 'add_center_of_mass_velocity', 'add_operational_frame', 'add_operational_frame_velocity',]:
wait_for_service("/hpp/target/" + srv)
......@@ -179,6 +179,16 @@ class Supervisor(object):
return sot.control
def publishState (self, subsampling = 100):
if hasattr (self, "ros_publish_state"):
from dynamic_graph.ros import RosPublish
self.ros_publish_state = RosPublish ("ros_publish_state")
ros_publish_state.add ("vector", "state", "/sot_hpp/state")
plug (self.sotrobot.device.state, self.ros_publish_state.state)
self.sotrobot.device.after.addDownsampledSignal ("ros_publish_state.trigger", 100)
def _handleHppJoint (n, t):
type = t["type"]
if t["velocity"]: topic = "velocity/op_frame"
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment