<commit_before>#!/usr/bin/python

import roslib
roslib.load_manifest('tf')
roslib.load_manifest('rospy')
roslib.load_manifest('adl_pr2_log')
roslib.load_manifest('geometry_msgs')
roslib.load_manifest('hrl_lib')
import rospy, optparse, math, time
import numpy as np
import tf
import tf.transformations as tr
import cPickle as pkl
import hrl_lib.transforms as hrl_tr
import hrl_lib.util as ut
from adl_pr2_log.msg import WrenchPoseArrayStamped
from geometry_msgs.msg import TransformStamped
from geometry_msgs.msg import WrenchStamped
from geometry_msgs.msg import Pose



def log_parse():
	parser = optparse.OptionParser('Input the source frame name \
		and the target frame name')

	parser.add_option("-t", "--tool", action="store", type="string",\
		dest="tool_frame", default="l_gripper_tool_frame")
	(options, args) = parser.parse_args()

	return options.tool_frame


class posearray_wrench_publisher():
	def __init__(self):
		self.tool_frame = '/'+log_parse()
#		self.head_frame = '/ellipse_frame'
		self.head_frame = '/'+log_parse()
		self.torso_frame = '/torso_lift_link'
		self.base_frame = '/base_link'
		ft_topic = '/netft_gravity_zeroing/wrench_zeroed'

		rospy.init_node('adl_poses_wrench', anonymous = True)
		self.pub = rospy.Publisher('/adl_wrench_posearray',\
				WrenchPoseArrayStamped)
		self.tflistener = tf.TransformListener()
		self.force_sub = rospy.Subscriber(ft_topic, WrenchStamped, self.listen_cb)
		self.msg = WrenchPoseArrayStamped()
		self.tool_pose = Pose()
		self.head_pose = Pose()
		self.torso_pose = Pose()

		self.tool_p = [0.,0.,0.]
		self.tool_q = [0.,0.,0.,0.]
		self.head_p = [0.,0.,0.]
		self.head_q = [0.,0.,0.,0.]
		self.torso_p = [0.,0.,0.]
		self.torso_q = [0.,0.,0.,0.]


	def listen_cb(self, f_msg):
		self.tool_p, self.tool_q = self.tflistener.lookupTransform\
			(self.torso_frame, self.tool_frame, rospy.Time(0))
		self.head_p, self.head_q = self.tflistener.lookupTransform\
			(self.torso_frame, self.head_frame, rospy.Time(0))
		self.torso_p, self.head_q = self.tflistener.lookupTransform\
			(self.base_frame, self.torso_frame, rospy.Time(0))
		self.msg.header.stamp = rospy.Time.now()
		self.msg.wrench = f_msg.wrench

#	poses[0] is the tool frame
		self.tool_pose.position.x = self.tool_p[0]
		self.tool_pose.position.y = self.tool_p[1]
		self.tool_pose.position.z = self.tool_p[2]
		self.tool_pose.orientation.x = self.tool_q[0]
		self.tool_pose.orientation.y = self.tool_q[1]
		self.tool_pose.orientation.z = self.tool_q[2]
		self.tool_pose.orientation.w = self.tool_q[3]
		self.msg.poses.append(self.tool_pose)
#	poses[1] is the head frame
		self.head_pose.position.x = self.head_p[0]
		self.head_pose.position.y = self.head_p[1]
		self.head_pose.position.z = self.head_p[2]
		self.head_pose.orientation.x = self.head_q[0]
		self.head_pose.orientation.y = self.head_q[1]
		self.head_pose.orientation.z = self.head_q[2]
		self.head_pose.orientation.w = self.head_q[3]
		self.msg.poses.append(self.head_pose)
#	poses[2] is the tool frame
		self.torso_pose.position.x = self.torso_p[0]
		self.torso_pose.position.y = self.torso_p[1]
		self.torso_pose.position.z = self.torso_p[2]
		self.torso_pose.orientation.x = self.torso_q[0]
		self.torso_pose.orientation.y = self.torso_q[1]
		self.torso_pose.orientation.z = self.torso_q[2]
		self.torso_pose.orientation.w = self.torso_q[3]
		self.msg.poses.append(self.torso_pose)

if __name__ == '__main__':
	data = posearray_wrench_publisher()
	rospy.sleep(1)
	while not rospy.is_shutdown():
		data.pub.publish(data.msg)
		rospy.sleep(1/100.)<commit_msg>move lookupTransform out of the callback function<commit_after>#!/usr/bin/python

import roslib
roslib.load_manifest('tf')
roslib.load_manifest('rospy')
roslib.load_manifest('adl_pr2_log')
roslib.load_manifest('geometry_msgs')
roslib.load_manifest('hrl_lib')
import rospy, optparse, math, time
import numpy as np
import tf
import tf.transformations as tr
import cPickle as pkl
import hrl_lib.transforms as hrl_tr
import hrl_lib.util as ut
from adl_pr2_log.msg import WrenchPoseArrayStamped
from geometry_msgs.msg import TransformStamped
from geometry_msgs.msg import WrenchStamped
from geometry_msgs.msg import Pose



def log_parse():
	parser = optparse.OptionParser('Input the source frame name \
		and the target frame name')

	parser.add_option("-t", "--tool", action="store", type="string",\
		dest="tool_frame", default="l_gripper_tool_frame")
	(options, args) = parser.parse_args()

	return options.tool_frame


class posearray_wrench_publisher():
	def __init__(self):
		self.tool_frame = '/'+log_parse()
#		self.head_frame = '/ellipse_frame'
		self.head_frame = '/'+log_parse()
		self.torso_frame = '/torso_lift_link'
		self.base_frame = '/base_link'
		ft_topic = '/netft_gravity_zeroing/wrench_zeroed'

		rospy.init_node('adl_poses_wrench', anonymous = True)
		self.pub = rospy.Publisher('/adl_wrench_posearray',\
				WrenchPoseArrayStamped)
		self.tflistener = tf.TransformListener()
		self.force_sub = rospy.Subscriber(ft_topic, WrenchStamped, self.force_cb)
		self.msg = WrenchPoseArrayStamped()
		self.tool_pose = Pose()
		self.head_pose = Pose()
		self.torso_pose = Pose()

		self.tool_p = [0.,0.,0.]
		self.tool_q = [0.,0.,0.,0.]
		self.head_p = [0.,0.,0.]
		self.head_q = [0.,0.,0.,0.]
		self.torso_p = [0.,0.,0.]
		self.torso_q = [0.,0.,0.,0.]


	def force_cb(self, f_msg):
		self.msg.wrench = f_msg.wrench		


	def pose_wrench_pub(self, f_msg):
		while not rospy.is_shutdown():
			self.tool_p, self.tool_q = self.tflistener.lookupTransform\
				(self.torso_frame, self.tool_frame, rospy.Time(0))
			self.head_p, self.head_q = self.tflistener.lookupTransform\
				(self.torso_frame, self.head_frame, rospy.Time(0))
			self.torso_p, self.head_q = self.tflistener.lookupTransform\
				(self.base_frame, self.torso_frame, rospy.Time(0))
			self.msg.header.stamp = rospy.Time.now()

#		poses[0] is the tool frame
			self.tool_pose.position.x = self.tool_p[0]
			self.tool_pose.position.y = self.tool_p[1]
			self.tool_pose.position.z = self.tool_p[2]
			self.tool_pose.orientation.x = self.tool_q[0]
			self.tool_pose.orientation.y = self.tool_q[1]
			self.tool_pose.orientation.z = self.tool_q[2]
			self.tool_pose.orientation.w = self.tool_q[3]
			self.msg.poses.append(self.tool_pose)
#		poses[1] is the head frame
			self.head_pose.position.x = self.head_p[0]
			self.head_pose.position.y = self.head_p[1]
			self.head_pose.position.z = self.head_p[2]
			self.head_pose.orientation.x = self.head_q[0]
			self.head_pose.orientation.y = self.head_q[1]
			self.head_pose.orientation.z = self.head_q[2]
			self.head_pose.orientation.w = self.head_q[3]
			self.msg.poses.append(self.head_pose)
#		poses[2] is the tool frame
			self.torso_pose.position.x = self.torso_p[0]
			self.torso_pose.position.y = self.torso_p[1]
			self.torso_pose.position.z = self.torso_p[2]
			self.torso_pose.orientation.x = self.torso_q[0]
			self.torso_pose.orientation.y = self.torso_q[1]
			self.torso_pose.orientation.z = self.torso_q[2]
			self.torso_pose.orientation.w = self.torso_q[3]
			self.msg.poses.append(self.torso_pose)

			self.pub.publish(self.msg)
			rospy.sleep(1/100.)


if __name__ == '__main__':
	data = posearray_wrench_publisher()
	rospy.sleep(1)
	try:
		data.pose_wrench_pub()
	except rospy.ROSInterruptException: pass

{'fraction_non_alphanumeric': 0.13893465129049973, 'fraction_numerical': 0.015101592531576058, 'mean_word_length': 4.518939393939394, 'pattern_counts': {'":': 0, '<': 3, '<?xml version=': 0, '>': 3, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 0, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'dir': 'git-commits-cleaned', 'id': '5281619', 'n_tokens_mistral': 3172, 'n_tokens_neox': 2928, 'n_words': 451}