Ejemplo n.º 1
0
    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
Ejemplo n.º 2
0
    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)
Ejemplo n.º 3
0
    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)
Ejemplo n.º 4
0
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'