def __init__(self):
"""Return a Customer object whose name is *name* and starting
balance is *balance*."""
# Ground truth:
self.last = pose_t()
self.last.utime = -2
self.last_est = pose_t()
self.last_est.utime = -2
self.most_recent_est = pose_t()
self.most_recent_est.utime = -2
self.parameterDDTThreshold = 0.25
self.parameterTimeElapsedThreshold = 10.0
def __init__(self):
"""Return a Customer object whose name is *name* and starting
balance is *balance*."""
# Ground truth:
self.last_est = pose_t()
self.last_est.utime = -2
self.counter =0
def callback(m):
rospy.loginfo(rospy.get_caller_id() + "I heard %f", m.orientation.x)
#rospy.loginfo(rospy.get_caller_id() + "I heard %s", data.data)
msg = pose_t()
msg.utime = 0
msg.pos = [0,0,0]
msg.orientation = [m.orientation.w, m.orientation.x, m.orientation.y, m.orientation.z]
lc.publish('POSE_BODY_ALT', msg.encode() )
def callback(m):
rospy.loginfo(rospy.get_caller_id() + "I heard %f", m.orientation.x)
#rospy.loginfo(rospy.get_caller_id() + "I heard %s", data.data)
msg = pose_t()
msg.utime = 0
msg.pos = [0, 0, 0]
msg.orientation = [
m.orientation.w, m.orientation.x, m.orientation.y, m.orientation.z
]
lc.publish('POSE_BODY_ALT', msg.encode())
import time
def euler_to_quat(rpy):
roll = rpy[0]
pitch = rpy[1]
yaw = rpy[2]
sy = math.sin(yaw * 0.5)
cy = math.cos(yaw * 0.5)
sp = math.sin(pitch * 0.5)
cp = math.cos(pitch * 0.5)
sr = math.sin(roll * 0.5)
cr = math.cos(roll * 0.5)
w = cr * cp * cy + sr * sp * sy
x = sr * cp * cy - cr * sp * sy
y = cr * sp * cy + sr * cp * sy
z = cr * cp * sy - sr * sp * cy
return np.array([w, x, y, z])
lc = lcm.LCM()
print "Send POSE_BODY..."
msg = pose_t()
msg.utime = 0
msg.pos = [0, 0, 1.2]
msg.orientation = euler_to_quat([0, 17.0 * np.pi / 180.0, 0])
lc.publish("POSE_BODY", msg.encode())
"leftAnkleRoll", "rightHipYaw", "rightHipRoll", "rightHipPitch", "rightKneePitch", "rightAnklePitch", "rightAnkleRoll"]
elif robot_name == "multisense":
print "Multisense"
joint_names = ["hokuyo_joint"]
lc = lcm.LCM()
print "Sending EST_ROBOT_STATE for ", robot_name
msg = robot_state_t();
msg.utime = 0;
msg.joint_name = joint_names
msg.num_joints =len(msg.joint_name);
msg.joint_position = [0]*msg.num_joints;
msg.joint_velocity = [0]*msg.num_joints;
msg.joint_effort = [0]*msg.num_joints;
msg2 = pose_t()
msg2.utime = msg.utime
if mode == "static":
print "Send a single pose..."
msg.pose.translation.x = 0
msg.pose.translation.y = 0
msg.pose.translation.z = 1.2
msg.joint_position[0] = 0
orientation = euler_to_quat([0, 0.0*np.pi/180.0,0])
msg.pose.rotation.w = orientation[0]
msg.pose.rotation.x = orientation[1]
msg.pose.rotation.y = orientation[2]
msg.pose.rotation.z = orientation[3]
lc.publish("EST_ROBOT_STATE", msg.encode())
#!/usr/bin/python
import os,sys
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")
import math
import lcm
from bot_core.pose_t import pose_t
from bot_core.rigid_transform_t import rigid_transform_t
from drc_utils import *
msg = pose_t()
msg.utime = 0
msg.pos = (0, 0, 0.0)
msg.orientation = (1, 0, 0, 0)
lc = lcm.LCM()
#lc.publish("PRE_SPINDLE_TO_POST_SPINDLE", msg.encode())
msg = rigid_transform_t()
msg.utime = 0
msg.trans = (0, 0, 0.0)
msg.quat = euler_to_quat([0,0, math.pi])
lc = lcm.LCM()
lc.publish("PRE_SPINDLE_TO_POST_SPINDLE", msg.encode())
from std_msgs.msg import Int32
import tf
from tf import transformations
import lcm
from bot_core.pose_t import pose_t
import math
import numpy
lc = lcm.LCM()
import time
from geometry_msgs.msg import TwistStamped
msgout = pose_t()
i = 0.0
pub = rospy.Publisher('/pose_controller/twist', TwistStamped, queue_size=10)
rospy.init_node('talker', anonymous=True)
rate = rospy.Rate(10) # 10hz
while not rospy.is_shutdown():
msg = TwistStamped()
msg.header.frame_id = "base"
msg.header.stamp = rospy.Time.now()
msg.twist.linear.x = numpy.sin(i / 10)
pub.publish(msg)
msgout.utime = (i / 10) * 1E6
imageDir = fullBaseName
channels = re.compile(channels)
log = EventLog(fname, "r")
sys.stderr.write("Enforcing a distance of %f between images\n" % dist_thresh)
sys.stderr.write("opened % s, outputing to % s\n" % (fname, imageDir))
lc = lcm.LCM()
msgCount = 0
statusMsg = ""
startTime = 0
imageChannels = []
lastPose =pose_t()
lastPose.utime =-1;
imageFnames = {}
imagePoses = {}
imageMeta = {}
imagesWritten =0
for e in log:
if msgCount == 0:
startTime = e.timestamp
if e.channel=="POSE":
lastPose = pose_t.decode(e.data);
elif lastPose.utime<0:
continue
]
elif robot_name == "multisense":
print "Multisense"
joint_names = ["hokuyo_joint"]
lc = lcm.LCM()
print "Sending EST_ROBOT_STATE for ", robot_name
msg = robot_state_t()
msg.utime = 0
msg.joint_name = joint_names
msg.num_joints = len(msg.joint_name)
msg.joint_position = [0] * msg.num_joints
msg.joint_velocity = [0] * msg.num_joints
msg.joint_effort = [0] * msg.num_joints
msg2 = pose_t()
msg2.utime = msg.utime
if mode == "static":
print "Send a single pose..."
msg.pose.translation.x = 0
msg.pose.translation.y = 0
msg.pose.translation.z = 1.2
msg.joint_position[0] = 0
orientation = euler_to_quat([0, 0.0 * np.pi / 180.0, 0])
msg.pose.rotation.w = orientation[0]
msg.pose.rotation.x = orientation[1]
msg.pose.rotation.y = orientation[2]
msg.pose.rotation.z = orientation[3]
lc.publish("EST_ROBOT_STATE", msg.encode())
imageDir = fullBaseName
channels = re.compile(channels)
log = EventLog(fname, "r")
sys.stderr.write("Enforcing a distance of %f between images\n" % dist_thresh)
sys.stderr.write("opened % s, outputing to % s\n" % (fname, imageDir))
lc = lcm.LCM()
msgCount = 0
statusMsg = ""
startTime = 0
imageChannels = []
lastPose = pose_t()
lastPose.utime = -1
imageFnames = {}
imagePoses = {}
imageMeta = {}
imagesWritten = 0
for e in log:
if msgCount == 0:
startTime = e.timestamp
if e.channel == "POSE":
lastPose = pose_t.decode(e.data)
elif lastPose.utime < 0:
continue
