Commit cd962bb5 authored by Joseph Mirabel's avatar Joseph Mirabel Committed by Joseph Mirabel

Publish base_link position wrt odom in republish

parent d464dabc
......@@ -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" />
......
#!/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()
......
Markdown is supported
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