Python PoseConv.to_pose_msg方法代码示例

# 需要导入模块: from hrl_geom.pose_converter import PoseConv [as 别名]
# 或者: from hrl_geom.pose_converter.PoseConv import to_pose_msg [as 别名]
    def retreat_move(self, height, velocity, forced=False):
        if forced:
            self.is_forced_retreat = True
        cart_pos, cart_quat = self.current_ee_pose(self.ee_frame, '/ellipse_frame')
        ell_pos, ell_quat = self.ellipsoid.pose_to_ellipsoidal((cart_pos, cart_quat))
        #print "Retreat EP:", ell_pos
        latitude = ell_pos[0]
        if self.trim_retreat:
            latitude = min(latitude, TRIM_RETREAT_LATITUDE)
        #rospy.loginfo("[face_adls_manager] Retreating from current location.")

        retreat_pos = [latitude, ell_pos[1], height]
        retreat_pos = self.ellipsoid.enforce_bounds(retreat_pos)
        retreat_quat = [0,0,0,1]
        if forced:
            cart_path = self.ellipsoid.ellipsoidal_to_pose((retreat_pos, retreat_quat))
            cart_msg = [PoseConv.to_pose_msg(cart_path)]
        else:
            ell_path = self.ellipsoid.create_ellipsoidal_path(ell_pos,
                                                              ell_quat,
                                                              retreat_pos,
                                                              retreat_quat,
                                                              velocity=0.01,
                                                              min_jerk=True)
            cart_path = [self.ellipsoid.ellipsoidal_to_pose(ell_pose) for ell_pose in ell_path]
            cart_msg = [PoseConv.to_pose_msg(pose) for pose in cart_path]
            
        head = Header()
        head.frame_id = '/ellipse_frame'
        head.stamp = rospy.Time.now()
        pose_array = PoseArray(head, cart_msg)
        self.pose_traj_pub.publish(pose_array)

        self.is_forced_retreat = False

# 需要导入模块: from hrl_geom.pose_converter import PoseConv [as 别名]
# 或者: from hrl_geom.pose_converter.PoseConv import to_pose_msg [as 别名]
def create_base_marker(pose, id, color):
    marker = Marker()
    marker.header.frame_id = "base_link"
    marker.header.stamp = rospy.Time.now()
    marker.ns = "ar_servo"
    marker.id = id
    marker.pose = PoseConv.to_pose_msg(pose)
    marker.color = ColorRGBA(*(color + (1.0,)))
    marker.scale.x = 0.7; marker.scale.y = 0.7; marker.scale.z = 0.2
    return marker

# 需要导入模块: from hrl_geom.pose_converter import PoseConv [as 别名]
# 或者: from hrl_geom.pose_converter.PoseConv import to_pose_msg [as 别名]
 def stop_moving(self):
     """Send empty PoseArray to skin controller to stop movement"""
     for x in range(2):
         self.pose_traj_pub.publish(PoseArray()) #Send empty msg to skin controller
     cart_traj_msg = [PoseConv.to_pose_msg(self.current_ee_pose(self.ee_frame))]
     head = Header()
     head.frame_id = '/torso_lift_link'
     head.stamp = rospy.Time.now()
     pose_array = PoseArray(head, cart_traj_msg)
     self.pose_traj_pub.publish(pose_array)
     rospy.loginfo( "Sent stop moving commands!")

# 需要导入模块: from hrl_geom.pose_converter import PoseConv [as 别名]
# 或者: from hrl_geom.pose_converter.PoseConv import to_pose_msg [as 别名]
def create_arrow_marker(pose, m_id, color=ColorRGBA(1., 0., 0., 1.)):
    m = Marker()
    m.header.frame_id = "/base_link"
    m.header.stamp = rospy.Time.now()
    m.ns = "ell_pose_viz"
    m.id = m_id
    m.type = Marker.ARROW
    m.action = Marker.ADD
    m.scale = Vector3(0.19, 0.09, 0.02)
    m.color = color
    m.pose = PoseConv.to_pose_msg(pose)
    return m

# 需要导入模块: from hrl_geom.pose_converter import PoseConv [as 别名]
# 或者: from hrl_geom.pose_converter.PoseConv import to_pose_msg [as 别名]
    def create_ear_marker(self, pose, m_id, color=ColorRGBA(0., 1., 1., 1.)):
        m = Marker()
#m.header.frame_id = "/base_link"
        m.header.frame_id = "/ellipse_frame"
        m.header.stamp = rospy.Time.now()
        m.ns = "head_markers"
        m.id = m_id
        m.type = Marker.CYLINDER
        m.action = Marker.ADD
        m.scale = ear_scale
        m.color = color
        m.pose = PoseConv.to_pose_msg(pose)
        return m

# 需要导入模块: from hrl_geom.pose_converter import PoseConv [as 别名]
# 或者: from hrl_geom.pose_converter.PoseConv import to_pose_msg [as 别名]
    def local_input_cb(self, msg):
        self.force_monitor.update_activity()
        if self.is_forced_retreat:
            return
        self.stop_moving()
        button_press = msg.data 

        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))

        if button_press in ell_trans_params:
            self.publish_feedback(Messages.LOCAL_START % button_names_dict[button_press])
            change_trans_ep = ell_trans_params[button_press]
            goal_ell_pos = [curr_ell_pos[0]+change_trans_ep[0],
                            curr_ell_pos[1]+change_trans_ep[1],
                            curr_ell_pos[2]+change_trans_ep[2]]
            ell_path = self.ellipsoid.create_ellipsoidal_path(curr_ell_pos, curr_ell_quat,
                                                              goal_ell_pos, curr_ell_quat,
                                                              velocity=ELL_LOCAL_VEL, min_jerk=True)
        elif button_press in ell_rot_params:
            self.publish_feedback(Messages.LOCAL_START % button_names_dict[button_press])
            change_rot_ep = ell_rot_params[button_press]
            rot_quat = trans.quaternion_from_euler(*change_rot_ep)
            goal_ell_quat = trans.quaternion_multiply(curr_ell_quat, rot_quat)
            ell_path = self.ellipsoid.create_ellipsoidal_path(curr_ell_pos, curr_ell_quat,
                                                              curr_ell_pos, goal_ell_quat,
                                                              velocity=ELL_ROT_VEL, min_jerk=True)
        elif button_press == "reset_rotation":
            self.publish_feedback(Messages.ROT_RESET_START)
            ell_path = self.ellipsoid.create_ellipsoidal_path(curr_ell_pos, curr_ell_quat,
                                                              curr_ell_pos,(0.,0.,0.,1.),
                                                              velocity=ELL_ROT_VEL, min_jerk=True)
        else:
            rospy.logerr("[face_adls_manager] Unknown local ellipsoidal command")

        cart_traj = [self.ellipsoid.ellipsoidal_to_pose(pose) for pose in ell_path]
        cart_traj_msg = [PoseConv.to_pose_msg(pose) for pose in 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)
        self.force_monitor.update_activity()

# 需要导入模块: from hrl_geom.pose_converter import PoseConv [as 别名]
# 或者: from hrl_geom.pose_converter.PoseConv import to_pose_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_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]