示例#1
0
    def pixel23d_callback(self, msg):
        x, y, z = (msg.pose.position.x, msg.pose.position.y,
                   msg.pose.position.z)
        self.pix1_pub.publish(msg)
        quat = [
            msg.pose.orientation.x, msg.pose.orientation.y,
            msg.pose.orientation.z, msg.pose.orientation.w
        ]
        quat_rot = tf_trans.quaternion_from_euler(0.0, np.pi / 2.0, 0.0)
        q = tf_trans.quaternion_multiply(quat, quat_rot)
        (msg.pose.orientation.x, msg.pose.orientation.y,
         msg.pose.orientation.z, msg.pose.orientation.w) = q
        #msg_trans = self.tf_listener.transformPose("torso_lift_link", msg)
        #print msg_trans
        #return

        setup_goal = LinearMoveRelativeSetupGoal()
        setup_goal.goal_pose = msg
        setup_goal.approach_dist = 0.3
        self.pix2_pub.publish(setup_goal.goal_pose)
        self.move_setup_client.send_goal(setup_goal)
        self.move_setup_client.wait_for_result()
        setup_result = self.move_setup_client.get_result()
        if self.move_setup_client.get_state() == GoalStatus.ABORTED:
            print "No IK solution for setup"
            return
        return
        rospy.sleep(1.5)

        move_goal = LinearMoveRelativeGoal()
        move_goal.distance = 0.02
        move_goal.goal_pose = PoseStamped()
        move_goal.goal_pose.header.frame_id = msg.header.frame_id
        move_goal.goal_pose.header.stamp = rospy.Time.now()
        move_goal.goal_pose.pose.position = Point(x, y, z)
        move_goal.goal_pose.pose.orientation = Quaternion(*q)
        self.move_client.send_goal(move_goal)
        self.move_client.wait_for_result()
        move_result = self.move_client.get_result()
        if self.move_client.get_state() == GoalStatus.ABORTED:
            print "No IK solution for move"
            return
    def pixel23d_callback(self, msg):
        x, y, z = (msg.pose.position.x, msg.pose.position.y, msg.pose.position.z)
        self.pix1_pub.publish(msg)
        quat = [msg.pose.orientation.x, msg.pose.orientation.y, 
                msg.pose.orientation.z, msg.pose.orientation.w]
        quat_rot = tf_trans.quaternion_from_euler(0.0, np.pi/2.0, 0.0)
        q = tf_trans.quaternion_multiply(quat, quat_rot)
        (msg.pose.orientation.x, msg.pose.orientation.y, 
         msg.pose.orientation.z, msg.pose.orientation.w) = q
#msg_trans = self.tf_listener.transformPose("torso_lift_link", msg)
#print msg_trans
#return

        setup_goal = LinearMoveRelativeSetupGoal()
        setup_goal.goal_pose = msg
        setup_goal.approach_dist = 0.3
        self.pix2_pub.publish(setup_goal.goal_pose)
        self.move_setup_client.send_goal(setup_goal)
        self.move_setup_client.wait_for_result()
        setup_result = self.move_setup_client.get_result()
        if self.move_setup_client.get_state() == GoalStatus.ABORTED:
            print "No IK solution for setup"
            return
        return
        rospy.sleep(1.5)

        move_goal = LinearMoveRelativeGoal()
        move_goal.distance = 0.02
        move_goal.goal_pose = PoseStamped()
        move_goal.goal_pose.header.frame_id = msg.header.frame_id
        move_goal.goal_pose.header.stamp = rospy.Time.now()
        move_goal.goal_pose.pose.position = Point(x, y, z)
        move_goal.goal_pose.pose.orientation = Quaternion(*q)
        self.move_client.send_goal(move_goal)
        self.move_client.wait_for_result()
        move_result = self.move_client.get_result()
        if self.move_client.get_state() == GoalStatus.ABORTED:
            print "No IK solution for move"
            return
示例#3
0
    def touch_face_at_pose(self, pose_stamped):
        setup_goal = LinearMoveRelativeSetupGoal()
        setup_goal.goal_pose = pose_stamped
        setup_goal.approach_dist = 0.3
        self.pix2_pub.publish(setup_goal.goal_pose)
        self.move_setup_client.send_goal(setup_goal)
        self.move_setup_client.wait_for_result()
        setup_result = self.move_setup_client.get_result()
        if self.move_setup_client.get_state() == GoalStatus.ABORTED:
            print "No IK solution for setup"
            return
        rospy.sleep(1.5)

        move_goal = LinearMoveRelativeGoal()
        move_goal.distance = 0.02
        move_goal.goal_pose = pose_stamped
        move_goal.goal_pose.header.stamp = rospy.Time.now()
        self.move_client.send_goal(move_goal)
        self.move_client.wait_for_result()
        move_result = self.move_client.get_result()
        if self.move_client.get_state() == GoalStatus.ABORTED:
            print "No IK solution for move"
            return
示例#4
0
    def touch_face_at_pose(self, pose_stamped):
        setup_goal = LinearMoveRelativeSetupGoal()
        setup_goal.goal_pose = pose_stamped
        setup_goal.approach_dist = 0.3
        self.pix2_pub.publish(setup_goal.goal_pose)
        self.move_setup_client.send_goal(setup_goal)
        self.move_setup_client.wait_for_result()
        setup_result = self.move_setup_client.get_result()
        if self.move_setup_client.get_state() == GoalStatus.ABORTED:
            print "No IK solution for setup"
            return
        rospy.sleep(1.5)

        move_goal = LinearMoveRelativeGoal()
        move_goal.distance = 0.02
        move_goal.goal_pose = pose_stamped
        move_goal.goal_pose.header.stamp = rospy.Time.now()
        self.move_client.send_goal(move_goal)
        self.move_client.wait_for_result()
        move_result = self.move_client.get_result()
        if self.move_client.get_state() == GoalStatus.ABORTED:
            print "No IK solution for move"
            return