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 ...@@ -5,28 +5,37 @@ from dynamic_graph_bridge_msgs.msg import Vector
from sensor_msgs.msg import JointState from sensor_msgs.msg import JointState
rospy.init_node('sot_reemitter', anonymous=True) 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() 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): def jointreceived(jstates):
aJS.header.seq = seqnb global seqnb
pub.publish(hello_str)
seqnb = seqnb+1 seqnb = seqnb+1
seqnb.stamp = ros::Time::now() aJS.header.seq = seqnb
seqnb.frameid = "base_link" aJS.header.stamp = rospy.Time.now()
seqnb.name = "joint_state" aJS.header.frame_id = "base_link"
seqnb.position = jstates.data[6:] aJS.name = jointnames
seqnb.velocity = [] aJS.position = jstates.data[6:]
seqnb.effort = [] aJS.velocity = []
aJS.effort = []
pub.publish(aJS)
def listener(): def listener():
rospy.Subscriber("/sot_hpp/state", Vector, jointreceived) rospy.Subscriber("/sot_hpp/state", Vector, jointreceived)
ros.spin() rospy.spin()
if __name__ == '__main__': if __name__ == '__main__':
try: try:
listener() listener()
except rospy.ROSInterruptException: except rospy.ROSInterruptException:
pass 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