Python PoseConv.to_pose_stamped_msg方法代码示例

# 需要导入模块: from hrl_geom.pose_converter import PoseConv [as 别名]
# 或者: from hrl_geom.pose_converter.PoseConv import to_pose_stamped_msg [as 别名]
def main():
    rospy.init_node("visualize_poses")
    pose_file = file(sys.argv[1], 'r')
    params = yaml.load(pose_file)
    ell_reg_bag = rosbag.Bag(sys.argv[2], 'r')
    for topic, ell_reg, ts in ell_reg_bag.read_messages():
        pass
    ell_space = EllipsoidSpace(E=ell_reg.E)

    pub_head_pose = rospy.Publisher("/head_center_test", PoseStamped)
    pub_arrows = rospy.Publisher("visualization_markers_array", MarkerArray)
    def create_tool_arrow():
        arrows = MarkerArray()
        color = ColorRGBA(0., 0., 1., 1.)
        for i, param in enumerate(params):
            ell_pos, ell_rot = params[param]
            _, ell_rot_mat = PoseConv.to_pos_rot([0]*3, ell_rot)
            cart_pose = PoseConv.to_homo_mat(ell_space.ellipsoidal_to_pose(*ell_pos))
            cart_pose[:3,:3] = cart_pose[:3,:3] * ell_rot_mat
            arrow = create_arrow_marker(cart_pose, i, color)
            arrow.header.stamp = rospy.Time.now()
            arrows.markers.append(arrow)
        return arrows
    r = rospy.Rate(10)
    while not rospy.is_shutdown():
        pub_head_pose.publish(PoseConv.to_pose_stamped_msg("/base_link", [0]*3, [0]*3))
        arrows = create_tool_arrow()
        pub_arrows.publish(arrows)
        r.sleep()

# 需要导入模块: from hrl_geom.pose_converter import PoseConv [as 别名]
# 或者: from hrl_geom.pose_converter.PoseConv import to_pose_stamped_msg [as 别名]
 def load_params(self, params):
     kinect_B_head = PoseConv.to_homo_mat(params.e_frame)
     base_B_kinect = self.kin_head.forward(base_link="base_link",
                                           end_link=self.kinect_frame)
     base_B_head = base_B_kinect * kinect_B_head
     self.head_center = PoseConv.to_pose_stamped_msg("/base_link",base_B_head)
     self.ell_space = EllipsoidSpace()
     self.ell_space.load_ell_params(params.E, params.is_oblate, params.height)
     rospy.loginfo("Loaded ellispoidal parameters.")

# 需要导入模块: from hrl_geom.pose_converter import PoseConv [as 别名]
# 或者: from hrl_geom.pose_converter.PoseConv import to_pose_stamped_msg [as 别名]
 def _run_traj(self, traj, blocking=True):
     self.start_pub.publish(
             PoseConv.to_pose_stamped_msg("/torso_lift_link", traj[0]))
     self.end_pub.publish(
             PoseConv.to_pose_stamped_msg("/torso_lift_link", traj[-1]))
     # make sure traj beginning is close to current end effector position
     init_pos_tolerance = rospy.get_param("~init_pos_tolerance", 0.05)
     init_rot_tolerance = rospy.get_param("~init_rot_tolerance", np.pi/12)
     ee_pos, ee_rot = PoseConv.to_pos_rot(self.arm.get_end_effector_pose())
     _, rot_diff = PoseConv.to_pos_euler((ee_pos, ee_rot * traj[0][1].T))
     pos_diff = np.linalg.norm(ee_pos - traj[0][0])
     if pos_diff > init_pos_tolerance:
         rospy.logwarn("[controller_base] End effector too far from current position. " + 
                       "Pos diff: %.3f, Tolerance: %.3f" % (pos_diff, init_pos_tolerance))
         return False
     if np.linalg.norm(rot_diff) > init_rot_tolerance:
         rospy.logwarn("[controller_base] End effector too far from current rotation. " + 
                       "Rot diff: %.3f, Tolerance: %.3f" % (np.linalg.norm(rot_diff), 
                                                            init_rot_tolerance))
         return False
     return self.execute_cart_traj(self.arm, traj, self.time_step, blocking=blocking)

# 需要导入模块: from hrl_geom.pose_converter import PoseConv [as 别名]
# 或者: from hrl_geom.pose_converter.PoseConv import to_pose_stamped_msg [as 别名]
    def init_reg_cb(self, req):
        # TODO REMOVE THIS FACE SIDE MESS
        self.publish_feedback("Performing Head Registration. Please Wait.")
        self.face_side = rospy.get_param("~face_side1", 'r')
        bag_str = self.reg_dir + '/' + '_'.join([self.subject, self.face_side, "head_transform"]) + ".bag"
        rospy.loginfo("[%s] Loading %s" %(rospy.get_name(), bag_str))
        try:
            bag = rosbag.Bag(bag_str, 'r')
            for topic, msg, ts in bag.read_messages():
                head_tf = msg
            assert (head_tf is not None), "Error reading head transform bagfile"
            bag.close()
        except Exception as e:
            self.publish_feedback("Registration failed: Error loading saved registration.")
            rospy.logerr("[%s] Cannot load registration parameters from %s:\r\n%s" %
                         (rospy.get_name(), bag_str, e))
            return (False, PoseStamped())

        if self.face_side == 'r':
            head_registration = self.head_registration_r
        else:
            head_registration = self.head_registration_l
        try:
            rospy.loginfo("[%s] Requesting head registration for %s at pixel (%d, %d)." %(rospy.get_name(),
                                                                                          self.subject,
                                                                                          req.u, req.v))
            self.head_pc_reg = head_registration(req.u, req.v).reg_pose
            if ((self.head_pc_reg.pose.position == Point(0.0, 0.0, 0.0)) and
                (self.head_pc_reg.pose.orientation == Quaternion(0.0, 0.0, 0.0, 1.0))):
               raise rospy.ServiceException("Unable to find a good match.")
               self.head_pc_reg = None
        except rospy.ServiceException as se:
            self.publish_feedback("Registration failed: %s" %se)
            return (False, PoseStamped())

        pc_reg_mat = PoseConv.to_homo_mat(self.head_pc_reg)
        head_tf_mat = PoseConv.to_homo_mat(Transform(head_tf.transform.translation,
                                                     head_tf.transform.rotation))
        self.head_pose = PoseConv.to_pose_stamped_msg(pc_reg_mat**-1 * head_tf_mat)
        self.head_pose.header.frame_id = self.head_pc_reg.header.frame_id
        self.head_pose.header.stamp = self.head_pc_reg.header.stamp

        side = "right" if (self.face_side == 'r') else "left"
        self.publish_feedback("Registered head using %s cheek model, please check and confirm." %side)
#        rospy.loginfo('[%s] Head frame registered at:\r\n %s' %(rospy.get_name(), self.head_pose))
        self.test_pose.publish(self.head_pose)
        return (True, self.head_pose)

# 需要导入模块: from hrl_geom.pose_converter import PoseConv [as 别名]
# 或者: from hrl_geom.pose_converter.PoseConv import to_pose_stamped_msg [as 别名]
    def global_input_cb(self, msg):
        self.force_monitor.update_activity()
        if self.is_forced_retreat:
            return
        rospy.loginfo("[face_adls_manager] Performing global move.")
        if type(msg) == String:
            self.publish_feedback(Messages.GLOBAL_START %msg.data)
            goal_ell_pose = self.global_poses[msg.data]
        elif type(msg) == PoseStamped:
            try:
                self.tfl.waitForTransform(msg.header.frame_id, '/ellipse_frame', msg.header.stamp, rospy.Duration(8.0))
                goal_cart_pose = self.tfl.transformPose('/ellipse_frame', msg)
                goal_cart_pos, goal_cart_quat = PoseConv.to_pos_quat(goal_cart_pose)
                flip_quat = trans.quaternion_from_euler(-np.pi/2, np.pi, 0)
                goal_cart_quat_flipped = trans.quaternion_multiply(goal_cart_quat, flip_quat)
                goal_cart_pose = PoseConv.to_pose_stamped_msg('/ellipse_frame', (goal_cart_pos, goal_cart_quat_flipped))
                self.publish_feedback('Moving around ellipse to clicked position).')
                goal_ell_pose = self.ellipsoid.pose_to_ellipsoidal(goal_cart_pose)
            except (LookupException, ConnectivityException, ExtrapolationException, Exception) as e:
                rospy.logwarn("[face_adls_manager] TF Failure getting clicked position in ellipse_frame:\r\n %s" %e)
                return

        curr_cart_pos, curr_cart_quat = self.current_ee_pose(self.ee_frame, '/ellipse_frame')
        curr_ell_pos, curr_ell_quat = self.ellipsoid.pose_to_ellipsoidal((curr_cart_pos, curr_cart_quat))
        retreat_ell_pos = [curr_ell_pos[0], curr_ell_pos[1], RETREAT_HEIGHT]
        retreat_ell_quat = trans.quaternion_multiply(self.gripper_rot, [1.,0.,0.,0.])
        approach_ell_pos = [goal_ell_pose[0][0], goal_ell_pose[0][1], RETREAT_HEIGHT]
        approach_ell_quat = trans.quaternion_multiply(self.gripper_rot, goal_ell_pose[1])
        final_ell_pos = [goal_ell_pose[0][0], goal_ell_pose[0][1], goal_ell_pose[0][2] - HEIGHT_CLOSER_ADJUST]
        final_ell_quat = trans.quaternion_multiply(self.gripper_rot, goal_ell_pose[1])
        
        cart_ret_pose = self.ellipsoid.ellipsoidal_to_pose((retreat_ell_pos, retreat_ell_quat))
        cart_ret_msg = PoseConv.to_pose_stamped_msg('ellipse_frame',cart_ret_pose)

        cart_app_pose = self.ellipsoid.ellipsoidal_to_pose((approach_ell_pos, approach_ell_quat))
        cart_app_msg = PoseConv.to_pose_stamped_msg('ellipse_frame',cart_app_pose)

        cart_goal_pose = self.ellipsoid.ellipsoidal_to_pose((final_ell_pos, final_ell_quat))
        cart_goal_msg = PoseConv.to_pose_stamped_msg('ellipse_frame',cart_goal_pose)

        retreat_ell_traj = self.ellipsoid.create_ellipsoidal_path(curr_ell_pos, curr_ell_quat,
                                                                  retreat_ell_pos, retreat_ell_quat,
                                                                  velocity=0.01, min_jerk=True)
        transition_ell_traj = self.ellipsoid.create_ellipsoidal_path(retreat_ell_pos, retreat_ell_quat,
                                                                     approach_ell_pos, approach_ell_quat,
                                                                     velocity=0.01, min_jerk=True)
        approach_ell_traj = self.ellipsoid.create_ellipsoidal_path(approach_ell_pos, approach_ell_quat,
                                                                   final_ell_pos, final_ell_quat,
                                                                   velocity=0.01, min_jerk=True)
        
        full_ell_traj = retreat_ell_traj + transition_ell_traj + approach_ell_traj
        full_cart_traj = [self.ellipsoid.ellipsoidal_to_pose(pose) for pose in full_ell_traj]
        cart_traj_msg = [PoseConv.to_pose_msg(pose) for pose in full_cart_traj]
        head = Header()
        head.frame_id = '/ellipse_frame'
        head.stamp = rospy.Time.now()
        pose_array = PoseArray(head, cart_traj_msg)
        self.pose_traj_pub.publish(pose_array)

        ps = PoseStamped()
        ps.header = head
        ps.pose = cart_traj_msg[0]
        
        self.force_monitor.update_activity()

# 需要导入模块: from hrl_geom.pose_converter import PoseConv [as 别名]
# 或者: from hrl_geom.pose_converter.PoseConv import to_pose_stamped_msg [as 别名]
    def enable_controller_cb(self, req):
        if req.enable:
            face_adls_modes = rospy.get_param('/face_adls_modes', None) 
            params = face_adls_modes[req.mode]
            self.face_side = rospy.get_param('/face_side', 'r') # TODO Make more general
            self.trim_retreat = req.mode == "shaving"
            self.flip_gripper = self.face_side == 'r' and req.mode == "shaving"
            bounds = params['%s_bounds' % self.face_side]
            self.ellipsoid.set_bounds(bounds['lat'], bounds['lon'], bounds['height'])#TODO: Change Back
            #self.ellipsoid.set_bounds((-np.pi,np.pi), (-np.pi,np.pi), (0,100))

            success, e_params = self.register_ellipse(req.mode, self.face_side)
            if not success:
                self.publish_feedback(Messages.NO_PARAMS_LOADED)
                return EnableFaceControllerResponse(False)

            reg_pose = PoseConv.to_pose_stamped_msg(e_params.e_frame)
            try:
                now = rospy.Time.now()
                reg_pose.header.stamp = now
                self.tfl.waitForTransform(reg_pose.header.frame_id, '/base_link',
                                          now, rospy.Duration(8.0))
                ellipse_frame_base = self.tfl.transformPose('/base_link', reg_pose)
            except (LookupException, ConnectivityException, ExtrapolationException, Exception) as e:
                rospy.logwarn("[face_adls_manager] TF Failure converting ellipse to base frame: %s" %e)

            self.ellipsoid.load_ell_params(ellipse_frame_base, e_params.E,
                                           e_params.is_oblate, e_params.height)
            global_poses_dir = rospy.get_param("~global_poses_dir", "")
            global_poses_file = params["%s_ell_poses_file" % self.face_side]
            global_poses_resolved = roslaunch.substitution_args.resolve_args(
                                            global_poses_dir + "/" + global_poses_file)
            self.global_poses = rosparam.load_file(global_poses_resolved)[0][0]
            self.global_move_poses_pub.publish(sorted(self.global_poses.keys()))

            #Check rotating gripper based on side of body
            if self.flip_gripper:
                self.gripper_rot = trans.quaternion_from_euler(np.pi, 0, 0)
                print "Rotating gripper by PI around x-axis"
            else:
                self.gripper_rot = trans.quaternion_from_euler(0, 0, 0)
                print "NOT Rotating gripper around x-axis"

            # check if arm is near head
#            cart_pos, cart_quat = self.current_ee_pose(self.ee_frame)
#            ell_pos, ell_quat = self.ellipsoid.pose_to_ellipsoidal((cart_pos, cart_quat))
#            equals = ell_pos == self.ellipsoid.enforce_bounds(ell_pos)
#            print ell_pos, equals
#            if self.ellipsoid._lon_bounds[0] >= 0 and ell_pos[1] >= 0:
#                arm_in_bounds =  np.all(equals)
#            else:
#                ell_lon_1 = np.mod(ell_pos[1], 2 * np.pi)
#                min_lon = np.mod(self.ellipsoid._lon_bounds[0], 2 * np.pi)
#                arm_in_bounds = (equals[0] and
#                        equals[2] and 
#                        (ell_lon_1 >= min_lon or ell_lon_1 <= self.ellipsoid._lon_bounds[1]))
            arm_in_bounds = True

            self.setup_force_monitor()

            success = True
            if not arm_in_bounds:
                self.publish_feedback(Messages.ARM_AWAY_FROM_HEAD)
                success = False

            #Switch back to l_arm_controller if necessary
            if self.controller_switcher.carefree_switch('l', '%s_arm_controller', reset=False):
                print "Controller switch to l_arm_controller succeeded"
                self.publish_feedback(Messages.ENABLE_CONTROLLER)
            else:
                print "Controller switch to l_arm_controller failed"
                success = False
            self.controller_enabled_pub.publish(Bool(success))
            req = EnableHapticMPCRequest()
            req.new_state = 'enabled'
            resp = self.enable_mpc_srv.call(req)
        else:
            self.stop_moving()
            self.controller_enabled_pub.publish(Bool(False))
            rospy.loginfo(Messages.DISABLE_CONTROLLER)
            req = EnableHapticMPCRequest()
            req.new_state = 'disabled'
            self.enable_mpc_srv.call(req)
            success = False
        return EnableFaceControllerResponse(success)

# 需要导入模块: from hrl_geom.pose_converter import PoseConv [as 别名]
# 或者: from hrl_geom.pose_converter.PoseConv import to_pose_stamped_msg [as 别名]
#!/usr/bin/python
import numpy as np
import roslib
roslib.load_manifest("hrl_geom")
import rospy
from hrl_geom.pose_converter import PoseConv

if __name__ == "__main__":
    rospy.init_node("test_poseconv")
    homo_mat = PoseConv.to_homo_mat([0., 1., 2.], [0., 0., np.pi/2])
    pose_msg = PoseConv.to_pose_msg(homo_mat)
    pos, quat = PoseConv.to_pos_quat(pose_msg)
    pose_stamped_msg = PoseConv.to_pose_stamped_msg("/base_link", pos, quat)
    pose_stamped_msg2 = PoseConv.to_pose_stamped_msg("/base_link", [pos, quat])
    tf_stamped = PoseConv.to_tf_stamped_msg("/new_link", pose_stamped_msg)
    p, R = PoseConv.to_pos_rot("/new_link", tf_stamped)
    for name in ["homo_mat", "pose_msg", "pos", "quat", "pose_stamped_msg", 
                 "pose_stamped_msg2", "tf_stamped", "p", "R"]:
        print "%s: \n" % name, locals()[name]

# 需要导入模块: from hrl_geom.pose_converter import PoseConv [as 别名]
# 或者: from hrl_geom.pose_converter.PoseConv import to_pose_stamped_msg [as 别名]
 def set_ep(self, pose):
     cep_pose_stmp = PoseConv.to_pose_stamped_msg('torso_lift_link', pose)
     self.command_pose_pub.publish(cep_pose_stmp)

# 需要导入模块: from hrl_geom.pose_converter import PoseConv [as 别名]
# 或者: from hrl_geom.pose_converter.PoseConv import to_pose_stamped_msg [as 别名]
def main():
    if len(sys.argv) < 3:
        print 'grab_cbs_auto cb_config.yaml output_bag.bag'
        return
    rospy.init_node("grab_cbs")

    f = file(sys.argv[1], 'r')
    cb_config = yaml.safe_load(f.read())
    print cb_config
    f.close()
    is_kinect = rospy.get_param("/grab_cbs/is_kinect", True)

    # load cb stuff
    chessboard = ChessboardInfo()
    chessboard.n_cols = cb_config['cols'] # 6
    chessboard.n_rows = cb_config['rows'] # 7
    chessboard.dim = cb_config['dim'] # 0.0258 
    calib = Calibrator([chessboard])
    bridge = CvBridge()

    l = DataListener(is_kinect, bridge, calib)
    tf_list = tf.TransformListener()

    cb_knowns = []
    for j in range(chessboard.n_cols):
        for i in range(chessboard.n_rows):
            cb_knowns.append((chessboard.dim*i, chessboard.dim*j, 0.0))
        
    bag = rosbag.Bag(sys.argv[2], 'w')
    i = 0
    while not rospy.is_shutdown():
        if raw_input("Press enter to take CB, type 'q' to quit: ") == "q":
            break

        tries = 0
        while not rospy.is_shutdown() and tries < 3:
            corners = l.wait_for_new(5.)
            if corners is None:
                print "No corners detected"
                tries += 1
                continue
            corners_2d = np.uint32(np.round(corners)).tolist()
            corners_3d = []
            if is_kinect:
                for x,y,z in read_points(l.cur_pc, field_names=['x', 'y', 'z'], uvs=corners_2d):
                    corners_3d.append((x,y,z))
                frame_id = l.cur_pc.header
            else:
                obj_pts = cv.fromarray(np.array(cb_knowns))
                img_pts = cv.fromarray(np.array(corners))
                K = cv.fromarray(np.reshape(l.cam_info.K,(3,3)))
                D = cv.fromarray(np.array([l.cam_info.D]))
                R_vec = cv.fromarray(np.zeros((3,1)))
                t = cv.fromarray(np.zeros((3,1)))
                cv.FindExtrinsicCameraParams2(obj_pts, img_pts, K, D, R_vec, t)
                R_mat = cv.fromarray(np.zeros((3,3)))
                cv.Rodrigues2(R_vec, R_mat)
                T = PoseConv.to_homo_mat(np.mat(np.asarray(t)).T.A.tolist(), 
                                         np.mat(np.asarray(R_mat)).A.tolist())
                cb_knowns_mat = np.vstack((np.mat(cb_knowns).T, np.mat(np.ones((1, len(cb_knowns))))))
                corners_3d = np.array((T * cb_knowns_mat)[:3,:].T)
                frame_id = l.cur_img.header
            print corners_3d
            if np.any(np.isnan(corners_3d)):
                print "Pointcloud malformed"
                tries += 1
                continue
            now = rospy.Time.now()
            corners_pc = create_cloud_xyz32(frame_id, corners_3d)
            try:
                pose = tf_list.lookupTransform('/base_link', '/ee_link', rospy.Time(0))
            except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
                print "TF screwed up..."
                continue
            bag.write("/pose", PoseConv.to_pose_stamped_msg('/base_link', pose), now)
            bag.write("/pc", corners_pc, now)
            print "Wrote pose/CB to bag file"
            break
        i += 1
    bag.close()