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
cd962bb5
Commit
cd962bb5
authored
Jun 20, 2018
by
Joseph Mirabel
Committed by
Joseph Mirabel
Jun 20, 2018
Browse files
Publish base_link position wrt odom in republish
parent
d464dabc
Changes
2
Hide whitespace changes
Inline
Side-by-side
roscontrol_sot_talos/launch/display_sot.launch
View file @
cd962bb5
...
...
@@ -2,11 +2,11 @@
<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="roscontrol_sot_talos" name="republish" type="republish" args="$(arg input_topic) $(arg output_prefix)/joint_state
true
"/>
<node pkg="tf" type="static_transform_publisher"
name="base_link_broadcaster"
args="0 0 0 0 0 0 1
base_link
$(arg output_prefix)/
base_link
100" />
args="0 0 0 0 0 0 1
odom
$(arg output_prefix)/
odom
100" />
<node pkg="robot_state_publisher" type="robot_state_publisher" name="rob_st_pub" >
<remap from="/joint_states" to="$(arg output_prefix)/joint_state" />
...
...
roscontrol_sot_talos/scripts/republish
View file @
cd962bb5
#!/usr/bin/env python
# license removed for brevity
import
rospy
import
rospy
,
tf
from
dynamic_graph_bridge_msgs.msg
import
Vector
from
sensor_msgs.msg
import
JointState
import
sys
publish_root_wrt_odom
=
False
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"
if
len
(
sys
.
argv
)
>
3
:
publish_root_wrt_odom
=
(
sys
.
argv
[
3
].
lower
()
in
(
"true"
,
"on"
))
else
:
input_topic
=
"/sot_hpp/state"
...
...
@@ -20,13 +23,21 @@ pub = rospy.Publisher(output_topic, JointState, queue_size=10)
seqnb
=
0
aJS
=
JointState
()
if
publish_root_wrt_odom
:
if
output_topic
.
find
(
'/'
)
>=
0
:
output_prefix
=
output_topic
.
rsplit
(
'/'
,
1
)[
0
]
+
'/'
else
:
output_prefix
=
""
rospy
.
loginfo
(
"Will publish "
+
output_prefix
+
"base_link with respect to "
+
output_prefix
+
"odom"
)
jointnames
=
rospy
.
get_param
(
"/sot_controller/joint_names"
)
def
jointreceived
(
jstates
):
global
seqnb
seqnb
=
seqnb
+
1
time
=
rospy
.
Time
.
now
()
aJS
.
header
.
seq
=
seqnb
aJS
.
header
.
stamp
=
rospy
.
Time
.
now
()
aJS
.
header
.
stamp
=
time
aJS
.
header
.
frame_id
=
"base_link"
aJS
.
name
=
jointnames
aJS
.
position
=
jstates
.
data
[
6
:]
...
...
@@ -34,6 +45,15 @@ def jointreceived(jstates):
aJS
.
effort
=
[]
pub
.
publish
(
aJS
)
if
publish_root_wrt_odom
and
len
(
jstates
.
data
)
>
6
:
br
=
tf
.
TransformBroadcaster
()
br
.
sendTransform
(
jstates
.
data
[
0
:
3
],
tf
.
transformations
.
quaternion_from_euler
(
jstates
.
data
[
3
],
jstates
.
data
[
4
],
jstates
.
data
[
5
]),
time
,
output_prefix
+
"base_link"
,
output_prefix
+
"odom"
)
def
listener
():
rospy
.
Subscriber
(
input_topic
,
Vector
,
jointreceived
)
rospy
.
spin
()
...
...
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