def fill_pose(self, det_names, det_poses, det_confidences):
msg = PoseArray()
msg.header.frame_id = '/head_rgbd_sensor_rgb_frame'
msg.header.stamp = rospy.Time(0)
for idx in range(len(det_names)):
item = Pose()
item.position.x = det_poses[idx][0]
item.position.y = det_poses[idx][1]
item.position.z = det_poses[idx][2]
item.orientation.w = det_poses[idx][3]
item.orientation.x = det_poses[idx][4]
item.orientation.y = det_poses[idx][5]
item.orientation.z = det_poses[idx][6]
msg.poses.append(item)
self.pose_pub.publish(msg)
msg = get_posesResponse()
for idx in range(len(det_names)):
item = PoseWithConfidence()
item.name = det_names[idx]
item.confidence = det_confidences[idx]
item.pose = Pose()
det_pose = det_poses[idx]
item.pose.position.x = det_pose[0]
item.pose.position.y = det_pose[1]
item.pose.position.z = det_pose[2]
item.pose.orientation.w = det_pose[3]
item.pose.orientation.x = det_pose[4]
item.pose.orientation.y = det_pose[5]
item.pose.orientation.z = det_pose[6]
msg.poses.append(item)
return msg
def person_callback(self, msg):
a = PeopleTracker()
a.header = msg.header
a.min_distance = np.inf
a.min_distance_angle = 0
for t in msg.tracks:
b = np.uint8(t.track_id)
a.uuids.append(str(b))
person = Pose()
person.position = t.pose.pose.position
person.orientation = t.pose.pose.orientation
a.poses.append(person)
v = Vector3()
v.x = t.twist.twist.linear.x
v.y = t.twist.twist.linear.y
v.z = t.twist.twist.linear.z
a.velocities.append(v)
person = PoseStamped()
person.header = msg.header
#person.header.frame_id = self.target_frame
person.pose = t.pose.pose
if msg.header.frame_id != self.target_frame:
try:
ti = self.listener.getLatestCommonTime(
self.target_frame, msg.header.frame_id)
msg.header.stamp = ti
transformed_person = self.listener.transformPose(
self.target_frame, person)
except (tf.Exception, tf.LookupException,
tf.ConnectivityException) as ex:
rospy.logerr("[" + rospy.get_name() + "]: " +
"Can't transform, reason: " + str(ex))
return None
x1 = transformed_person.pose.position.x
y1 = transformed_person.pose.position.y
z1 = transformed_person.pose.position.z
dis = round(
math.sqrt(math.pow(x1, 2) + math.pow(y1, 2) + math.pow(z1, 2)),
3)
a.distances.append(dis)
#ang = math.atan2(math.sqrt(math.pow(x1,2)+math.pow(y1,2)),z1)
ang = math.atan2(y1, x1)
a.angles.append(ang)
if dis < a.min_distance:
a.min_distance = dis
a.min_distance_angle = ang
self.pub.publish(a)
def person_callback(self, msg):
a = PeopleTracker()
#print("msg tracks: ")
#print(msg.tracks)
#for t in msg.header:
# for s in a.header:
# s.seq = t.seq
# s.stamp = t.stamp
# s.frame_id = t.frame_id
# print("Frame ID: ")
# print(s.frame_id)
a.header = msg.header
print("header: ")
print(a.header)
for t in msg.tracks:
b = np.uint8(t.track_id)
a.uuids.append(str(b)) #= b#'track_%08d' %(t.track_id)
print("track_id: ")
print(t.track_id)
print("uuids: ")
print(a.uuids)
print("poses: ")
#print(t.pose)
person = Pose()
#person.header = a.header
person.position = t.pose.pose.position
person.orientation = t.pose.pose.orientation
a.poses.append(person) # = t.pose
print(a.poses)
#for t1 in t.pose:
# for s in a.poses:
# for t2 in t1.pose:
# for t3 in t2.position:
# for s1 in s.position:
# s1.x = t3.x
# s1.y = t3.y
# s1.z = t3.z
# for t3 in t2.orientation:
# for s1 in s.orientation:
# s1.x = t3.x
# s1.y = t3.y
# s1.z = t3.z
# s1.w = t3.w
v = Vector3()
v.x = t.twist.twist.linear.x
v.y = t.twist.twist.linear.y
v.z = t.twist.twist.linear.z
a.velocities.append(v) # = t.twist
print("velocities: ")
print(a.velocities)
#for s in a.velocities:
# for t1 in t.twist:
# for t2 in t1.linear:
# s.x = t2.x
# s.y = t2.y
# s.z = t2.z
person = PoseStamped()
person.header = msg.header
print(type(t.pose.pose))
person.pose = t.pose.pose
print("person pose: ")
print(type(person))
#print(person.pose)
if msg.header.frame_id != self.target_frame:
try:
ti = self.listener.getLatestCommonTime(
self.target_frame, msg.header.frame_id)
msg.header.stamp = ti
transformed_person = self.listener.transformPose(
self.target_frame, person)
except (tf.Exception, tf.LookupException,
tf.ConnectivityException) as ex:
return None
x1 = transformed_person.pose.position.x
y1 = transformed_person.pose.position.y
z1 = transformed_person.pose.position.z
print("x: ")
print(x1)
print("y: ")
print(y1)
print("z: ")
print(z1)
dis = [
math.sqrt(math.pow(x1, 2) + math.pow(y1, 2) + math.pow(z1, 2))
] # = math.sqrt(math.pow(x,2)+math.pow(y,2)+math.pow(z,2))
a.distances.append(dis[0])
print("distances: ")
print(a.distances)
ang = [
math.atan2(math.sqrt(math.pow(x1, 2) + math.pow(y1, 2)), z1)
] # = math.atan2(math.sqrt(math.pow(x,2)+math.pow(y,2)),z)
a.angles.append(ang[0])
print("angles: ")
print(a.angles)
#self.pub.publish(a)
#print("I just published")
#break
#a.distances.append(math.sqrt(math.pow(x,2)+math.pow(y,2)+math.pow(z,2)))
#a.angles.append(math.atan2(math.sqrt(math.pow(x,2)+math.pow(y,2)),z))
a.min_distance = min(a.distances)
print("min distance: ")
print(a.min_distance)
i = 0
for d in a.distances:
if d == a.min_distance:
break
else:
i = i + 1
a.min_distance_angle = a.angles[i]
print("min distance angle: ")
print(a.min_distance_angle)
print("I am publishing People Tracker object a.")
self.pub.publish(a)
gc.importRobot('fetch_gripper')
pose = Pose()
pose.orientation.w = 1.0
gc.importGraspableBody(
"/home/vlad/Downloads/ycb_meshes/spam_12oz/meshes/spam_12oz.ply", pose)
graspit_grasps = gc.planGrasps(graspable_body_id=0)
# add Spam to MoveIt
import geometry_msgs.msg
import world_manager
from geometry_msgs.msg import Pose, Point, Quaternion
pose = geometry_msgs.msg.PoseStamped()
pose.header.frame_id = '/base_link'
pose.pose = Pose(Point(0.042, 0.384, 0.826), Quaternion(0, 0, 0, 1))
world_manager.world_manager_client.add_mesh(
'spam', '/home/vlad/Downloads/spam_12oz.dae', pose)
### Using MoveIt to move arm -- this works by itsels, without using CURPP. The arm moves to this location.
import rospy
from moveit_msgs.msg import MoveItErrorCodes
from moveit_python import MoveGroupInterface, PlanningSceneInterface
from geometry_msgs.msg import PoseStamped, Pose, Point, Quaternion
rospy.init_node("hi")
move_group_arm = MoveGroupInterface("arm_with_torso", "base_link")
# This is the wrist link not the gripper itself
gripper_frame = 'wrist_roll_link'