home_dir =os.getenv("HOME")
#print home_dir
sys.path.append(home_dir + "/drc/software/build/lib/python2.7/site-packages")
sys.path.append(home_dir + "/drc/software/build/lib/python2.7/dist-packages")
#sys.path.append(home_dir + "/otherprojects/pronto-distro/build/lib/python2.7/site-packages")
#sys.path.append(home_dir + "/otherprojects/pronto-distro/build/lib/python2.7/dist-packages")
from bot_core.pose_t import pose_t
from bot_core.robot_state_t import robot_state_t
from bot_core.joint_state_t import joint_state_t
########################################################################################
def timestamp_now (): return int (time.time () * 1000000)
global joint_state
joint_state = joint_state_t()
def on_joint_state(channel, data):
global joint_state
joint_state = joint_state_t.decode(data)
def on_pose_body(channel, data):
if (joint_state.num_joints==0):
return
m = pose_t.decode(data)
o = robot_state_t()
home_dir =os.getenv("HOME")
#print home_dir
sys.path.append(home_dir + "/drc/software/build/lib/python2.7/site-packages")
sys.path.append(home_dir + "/drc/software/build/lib/python2.7/dist-packages")
#sys.path.append(home_dir + "/otherprojects/pronto-distro/build/lib/python2.7/site-packages")
#sys.path.append(home_dir + "/otherprojects/pronto-distro/build/lib/python2.7/dist-packages")
from bot_core.pose_t import pose_t
from bot_core.robot_state_t import robot_state_t
from bot_core.joint_state_t import joint_state_t
########################################################################################
def timestamp_now (): return int (time.time () * 1000000)
global joint_state
joint_state = joint_state_t()
joint_state.joint_name = ['lf_haa_joint', 'lf_hfe_joint', 'lf_kfe_joint', 'rf_haa_joint', 'rf_hfe_joint', 'rf_kfe_joint', 'lh_haa_joint', 'lh_hfe_joint', 'lh_kfe_joint', 'rh_haa_joint', 'rh_hfe_joint', 'rh_kfe_joint', 'ptu_pan', 'ptu_tilt']
joint_state.num_joints = len(joint_state.joint_name)
joint_state.joint_position = [0] * len(joint_state.joint_name)
joint_state.joint_velocity = [0] * len(joint_state.joint_name)
joint_state.joint_effort = [0] * len(joint_state.joint_name)
received_joint_state = False
skip_joint_angles = True
if skip_joint_angles:
received_joint_state = True
def on_joint_state(channel, data):
global joint_state, received_joint_state
from bot_core.pose_t import pose_t
from bot_core.robot_state_t import robot_state_t
from bot_core.joint_state_t import joint_state_t
from bot_core.vector_3d_t import vector_3d_t
from bot_core.position_3d_t import position_3d_t
from bot_core.twist_t import twist_t
from bot_core.quaternion_t import quaternion_t
from bot_core.twist_t import twist_t
from bot_core.force_torque_t import force_torque_t
########################################################################################
def timestamp_now (): return int (time.time () * 1000000)
global core_robot_state
core_robot_state = joint_state_t()
def on_core_robot_state(channel, data):
global core_robot_state
core_robot_state = joint_state_t.decode(data)
def on_pose_body(channel, data):
global core_robot_state, joint_names
if (core_robot_state.num_joints==0):
return
m = pose_t.decode(data)
o = robot_state_t()
o.utime = m.utime
from bot_core.vector_3d_t import vector_3d_t
from bot_core.position_3d_t import position_3d_t
from bot_core.twist_t import twist_t
from bot_core.quaternion_t import quaternion_t
from bot_core.twist_t import twist_t
from bot_core.force_torque_t import force_torque_t
########################################################################################
def timestamp_now():
return int(time.time() * 1000000)
global core_robot_state
core_robot_state = joint_state_t()
def on_core_robot_state(channel, data):
global core_robot_state
core_robot_state = joint_state_t.decode(data)
def on_pose_body(channel, data):
global core_robot_state, joint_names
if (core_robot_state.num_joints == 0):
return
m = pose_t.decode(data)
o = robot_state_t()
