Commit abdb87a0 authored by olivier stasse's avatar olivier stasse Committed by GitHub

Merge pull request #2 from jmirabel/master

Update republish + config for position control
parents d464dabc f8db9af3
......@@ -7,8 +7,8 @@ sot_controller:
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
]
map_rc_to_sot_device: { motor-angles: motor-angles ,
joint-angles: joint-angles, velocities: velocities,
torques: torques, cmd-joints: control, cmd-effort: control }
joint-angles: joint-angles, velocities: velocities, forces: forces, currents: currents,
torques: torques, cmd-joints: control, cmd-effort: control, accelerometer_0: accelerometer_0, gyrometer_0: gyrometer_0 }
position_control_init_pos:
- name: leg_left_1_joint
des_pos: 0.0
......
......@@ -8,8 +8,8 @@ sot_controller:
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
]
map_rc_to_sot_device: { motor-angles: motor-angles ,
joint-angles: joint-angles, velocities: velocities,
torques: torques, cmd-joints: control, cmd-effort: control }
joint-angles: joint-angles, velocities: velocities, forces: forces, currents: currents,
torques: torques, cmd-joints: control, cmd-effort: control, accelerometer_0: accelerometer_0, gyrometer_0: gyrometer_0 }
position_control_init_pos:
- name: leg_left_1_joint
des_pos: 0.0
......
......@@ -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
# Arg 1
input_topic = "/sot_hpp/state"
# Arg 2
output_topic = "/sot/joint_state"
# Arg 3
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"
else:
input_topic = "/sot_hpp/state"
if len(sys.argv)>3:
publish_root_wrt_odom = (sys.argv[3].lower() in ("true","on"))
rospy.init_node('sot_reemitter', anonymous=True)
pub = rospy.Publisher(output_topic, JointState, queue_size=10)
......@@ -20,13 +24,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 +46,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