Commit e28973e9 authored by Olivier Stasse's avatar Olivier Stasse

Add launch file to display the SoT output in rviz (servo off mode)

parent 6f677782
<launch>
<node pkg="roscontrol_sot_talos" name="republish" type="republish"/>
<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" />
<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" />
</node>
<node pkg="rviz" type="rviz" name="rviz" />
</launch>
......@@ -5,28 +5,37 @@ from dynamic_graph_bridge_msgs.msg import Vector
from sensor_msgs.msg import JointState
rospy.init_node('sot_reemitter', anonymous=True)
pub = rospy.Publisher('/sot/joint_state', String, queue_size=10)
pub = rospy.Publisher('/sot/joint_state', JointState, queue_size=10)
seqnb = 0
aJS = JointState()
jointnames = [
'leg_left_1_joint', 'leg_left_2_joint', 'leg_left_3_joint', 'leg_left_4_joint', 'leg_left_5_joint', 'leg_left_6_joint',
'leg_right_1_joint', 'leg_right_2_joint', 'leg_right_3_joint', 'leg_right_4_joint', 'leg_right_5_joint', 'leg_right_6_joint',
'torso_1_joint', 'torso_2_joint',
'arm_left_1_joint', 'arm_left_2_joint', 'arm_left_3_joint', 'arm_left_4_joint', 'arm_left_5_joint', 'arm_left_6_joint', 'arm_left_7_joint', 'gripper_left_joint','arm_right_1_joint', 'arm_right_2_joint', 'arm_right_3_joint', 'arm_right_4_joint', 'arm_right_5_joint', 'arm_right_6_joint', 'arm_right_7_joint', 'gripper_right_joint',
'head_1_joint', 'head_2_joint',
]
def jointreceived(jstates):
aJS.header.seq = seqnb
pub.publish(hello_str)
global seqnb
seqnb = seqnb+1
seqnb.stamp = ros::Time::now()
seqnb.frameid = "base_link"
seqnb.name = "joint_state"
seqnb.position = jstates.data[6:]
seqnb.velocity = []
seqnb.effort = []
aJS.header.seq = seqnb
aJS.header.stamp = rospy.Time.now()
aJS.header.frame_id = "base_link"
aJS.name = jointnames
aJS.position = jstates.data[6:]
aJS.velocity = []
aJS.effort = []
pub.publish(aJS)
def listener():
rospy.Subscriber("/sot_hpp/state", Vector, jointreceived)
ros.spin()
rospy.spin()
if __name__ == '__main__':
try:
listener()
except rospy.ROSInterruptException:
pass
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