Skip to content
Snippets Groups Projects
Commit 4c5f0495 authored by Joseph Mirabel's avatar Joseph Mirabel Committed by Joseph Mirabel
Browse files

Make display_sot.launch more flexible.

parent 3a299285
No related branches found
No related tags found
1 merge request!1Enhance debugging tools + fix gazebo simulation.
<launch>
<node pkg="roscontrol_sot_talos" name="republish" type="republish"/>
<arg name="input_topic" default="/sot_hpp/state"/>
<arg name="output_prefix" default="/sot"/>
<node pkg="roscontrol_sot_talos" name="republish" type="republish" args="$(arg input_topic) $(arg output_prefix)/joint_state"/>
<node pkg="tf" type="static_transform_publisher"
name="base_link_broadcaster"
args="0 0 0 0 0 0 1 base_link sot/base_link 100" />
args="0 0 0 0 0 0 1 base_link $(arg output_prefix)/base_link 100" />
<node pkg="robot_state_publisher" type="robot_state_publisher" name="rob_st_pub" >
<remap from="/joint_states" to="/sot/joint_state" />
<param name="tf_prefix" value="/sot" />
<remap from="/joint_states" to="$(arg output_prefix)/joint_state" />
<param name="tf_prefix" value="$(arg output_prefix)" />
</node>
<node pkg="rviz" type="rviz" name="rviz" />
......
......@@ -3,9 +3,19 @@
import rospy
from dynamic_graph_bridge_msgs.msg import Vector
from sensor_msgs.msg import JointState
import sys
if len(sys.argv)>1:
input_topic = sys.argv[1]
if len(sys.argv)>2:
output_topic = sys.argv[2]
else:
output_topic = "/sot/joint_state"
else:
input_topic = "/sot_hpp/state"
rospy.init_node('sot_reemitter', anonymous=True)
pub = rospy.Publisher('/sot/joint_state', JointState, queue_size=10)
pub = rospy.Publisher(output_topic, JointState, queue_size=10)
seqnb = 0
aJS = JointState()
......@@ -25,7 +35,7 @@ def jointreceived(jstates):
pub.publish(aJS)
def listener():
rospy.Subscriber("/sot_hpp/state", Vector, jointreceived)
rospy.Subscriber(input_topic, Vector, jointreceived)
rospy.spin()
if __name__ == '__main__':
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment