Skip to content
GitLab
Projects
Groups
Snippets
/
Help
Help
Support
Community forum
Keyboard shortcuts
?
Submit feedback
Contribute to GitLab
Sign in / Register
Toggle navigation
Menu
Open sidebar
Stack Of Tasks
talos-metapkg-ros-control-sot
Commits
4c5f0495
Commit
4c5f0495
authored
Jun 05, 2018
by
Joseph Mirabel
Committed by
Joseph Mirabel
Jun 05, 2018
Browse files
Make display_sot.launch more flexible.
parent
3a299285
Changes
2
Hide whitespace changes
Inline
Side-by-side
roscontrol_sot_talos/launch/display_sot.launch
View file @
4c5f0495
<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" />
...
...
roscontrol_sot_talos/scripts/republish
View file @
4c5f0495
...
...
@@ -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__'
:
...
...
Write
Preview
Supports
Markdown
0%
Try again
or
attach a new file
.
Cancel
You are about to add
0
people
to the discussion. Proceed with caution.
Finish editing this message first!
Cancel
Please
register
or
sign in
to comment