Python msg.PoseWithCovarianceStamped方法代码示例

# 需要导入模块: from geometry_msgs import msg [as 别名]
# 或者: from geometry_msgs.msg import PoseWithCovarianceStamped [as 别名]
def __pub_initial_position(self, x, y, theta):
        """
        Publishing new initial position (x, y, theta) --> for localization
        :param x x-position of the robot
        :param y y-position of the robot
        :param theta theta-position of the robot
        """
        initpose = PoseWithCovarianceStamped()
        initpose.header.stamp = rospy.get_rostime()
        initpose.header.frame_id = "map"
        initpose.pose.pose.position.x = x
        initpose.pose.pose.position.y = y
        quaternion = self.__yaw_to_quat(theta)

        initpose.pose.pose.orientation.w = quaternion[0]
        initpose.pose.pose.orientation.x = quaternion[1]
        initpose.pose.pose.orientation.y = quaternion[2]
        initpose.pose.pose.orientation.z = quaternion[3]
        self.__initialpose_pub.publish(initpose)
        return 

# 需要导入模块: from geometry_msgs import msg [as 别名]
# 或者: from geometry_msgs.msg import PoseWithCovarianceStamped [as 别名]
def __init__(self):
        self.map = None
        self.start = None
        self.goal = None

        self.moves = [Move(0.1, 0),  # forward
                      Move(-0.1, 0),  # back
                      Move(0, 1.5708),  # turn left 90
                      Move(0, -1.5708)] # turn right 90
        self.robot = Robot(0.5, 0.5)
        self.is_working = False # Replace with mutex after all

        self.map_subscriber = rospy.Subscriber("map", OccupancyGrid, self.new_map_callback)
        self.start_subscriber = rospy.Subscriber("initialpose", PoseWithCovarianceStamped, self.new_start_callback)
        self.goal_subscriber = rospy.Subscriber("goal", PoseStamped, self.new_goal_callback)

        self.path_publisher = rospy.Publisher("trajectory", MarkerArray, queue_size=1)
        self.pose_publisher = rospy.Publisher("debug_pose", PoseStamped, queue_size=1)

        # what will be there. A module goes into variable. Isn't it too much memory consumption. Maybe I should assign function replan() to this variable?
        self.planner = planners.astar.replan 

# 需要导入模块: from geometry_msgs import msg [as 别名]
# 或者: from geometry_msgs.msg import PoseWithCovarianceStamped [as 别名]
def GetCurrentRobotPose(self,frame="map"):
        self.tfl.waitForTransform(frame, "base_link", rospy.Time(), rospy.Duration(1.0))
        (trans,rot) = self.tfl.lookupTransform(frame, "base_link", rospy.Time(0))
        
        """
        Remove all the rotation components except yaw
        """
        euler = tf.transformations.euler_from_quaternion(rot)
        rot = tf.transformations.quaternion_from_euler(0,0,euler[2])    
    
        current_pose = PoseWithCovarianceStamped()
        current_pose.header.stamp = rospy.get_rostime()
        current_pose.header.frame_id = frame
        current_pose.pose.pose = Pose(Point(trans[0], trans[1], 0.0), Quaternion(rot[0],rot[1],rot[2],rot[3]))
        
        return current_pose 

# 需要导入模块: from geometry_msgs import msg [as 别名]
# 或者: from geometry_msgs.msg import PoseWithCovarianceStamped [as 别名]
def __init__(self):
    self.lock = threading.Lock()
    self.sub_imu = rospy.Subscriber('robot_pose_ekf/odom_combined', PoseWithCovarianceStamped, self.imu_cb)
    self.last_imu_angle = 0
    self.imu_angle = 0
    while not rospy.is_shutdown():
      rospy.sleep(0.1)
      rospy.loginfo("imu_angle:"+str((self.imu_angle)*180/3.1415926)) 

# 需要导入模块: from geometry_msgs import msg [as 别名]
# 或者: from geometry_msgs.msg import PoseWithCovarianceStamped [as 别名]
def __init__(self):
        # Give the node a name
        rospy.init_node('odom_ekf', anonymous=False)

        # Publisher of type nav_msgs/Odometry
        self.ekf_pub = rospy.Publisher('output', Odometry, queue_size=5)
        
        # Wait for the /odom_combined topic to become available
        rospy.wait_for_message('input', PoseWithCovarianceStamped)
        
        # Subscribe to the /odom_combined topic
        rospy.Subscriber('input', PoseWithCovarianceStamped, self.pub_ekf_odom)
        
        rospy.loginfo("Publishing combined odometry on /odom_ekf") 

# 需要导入模块: from geometry_msgs import msg [as 别名]
# 或者: from geometry_msgs.msg import PoseWithCovarianceStamped [as 别名]
def _get_current_pose(self):

        """
        Gets the current pose of the base frame in the global frame
        """
        current_pose = None
        listener = tf.TransformListener()
        rospy.sleep(1.0)
        try:
            listener.waitForTransform(self.global_frame, self.base_frame, rospy.Time(), rospy.Duration(1.0))
        except:
            pass
        try:
            (trans,rot) = listener.lookupTransform(self.global_frame, self.base_frame, rospy.Time(0))

            pose_parts = [0.0] * 7
            pose_parts[0]  = trans[0]
            pose_parts[1]  = trans[1]
            pose_parts[2]  = 0.0
            euler = tf.transformations.euler_from_quaternion(rot)
            rot = tf.transformations.quaternion_from_euler(0,0,euler[2])
            pose_parts[3] = rot[0]
            pose_parts[4] = rot[1]
            pose_parts[5] = rot[2]
            pose_parts[6] = rot[3]

            current_pose = PoseWithCovarianceStamped()
            current_pose.header.stamp = rospy.get_rostime()
            current_pose.header.frame_id = self.global_frame
            current_pose.pose.pose = Pose(Point(pose_parts[0], pose_parts[1], pose_parts[2]), Quaternion(pose_parts[3],pose_parts[4],pose_parts[5],pose_parts[6]))
        except:
            rospy.loginfo("Could not get transform from %(1)s->%(2)s"%{"1":self.global_frame,"2":self.base_frame})

        return current_pose 

# 需要导入模块: from geometry_msgs import msg [as 别名]
# 或者: from geometry_msgs.msg import PoseWithCovarianceStamped [as 别名]
def __init__(self):
        self._use_lsm_for_odom = rospy.get_param('~use_lsm_for_odom',False)
        self._MsgData = Dynamics()
        self._MsgPub = rospy.Publisher('/movo/feedback/dynamics', Dynamics, queue_size=10)
        self._jointStatePub = rospy.Publisher('/movo/linear_actuator/joint_states', JointState, queue_size=10)
        self._jointStateMsg = JointState()
        self._jointStateMsg.name = ['linear_joint']
        self._MsgData.header.frame_id = ''
        self._jointStateMsg.header.frame_id = ''

        self._OdomData1 = Odometry()
        self._OdomData2 = Odometry()
        self._OdomPub1 = rospy.Publisher('/movo/feedback/wheel_odometry', Odometry, queue_size=10)
        if (False == self._use_lsm_for_odom):
            rospy.Subscriber('/movo/odometry/local_filtered', Odometry, self._update_odom_yaw)
        else:
            self._OdomPub2 = rospy.Publisher('/movo/odometry/local_filtered', Odometry, queue_size=10)
            self.has_recved_lsm = False
            rospy.Subscriber('/movo/lsm/pose', PoseWithCovarianceStamped, self._update_lsm_odom)     
        
        self._OdomData1.header.frame_id = 'odom'
        self._OdomData1.child_frame_id  = 'base_link'
        
        
        self._OdomData1.pose.covariance = [0.00017,0.0,0.0,0.0,0.0,0.0,
                                          0.0,0.00017,0.0,0.0,0.0,0.0,
                                          0.0,0.0,0.00017,0.0,0.0,0.0,
                                          0.0,0.0,0.0,0.00000,0.0,0.0,
                                          0.0,0.0,0.0,0.0,0.00000,0.0,
                                          0.0,0.0,0.0,0.0,0.0,0.00017]
             
        self._OdomData1.twist.covariance = [0.00017,0.0,0.0,0.0,0.0,0.0,
                                           0.0,0.00017,0.0,0.0,0.0,0.0,
                                           0.0,0.0,0.00017,0.0,0.0,0.0,
                                           0.0,0.0,0.0,0.00000,0.0,0.0,
                                           0.0,0.0,0.0,0.0,0.00000,0.0,
                                           0.0,0.0,0.0,0.0,0.0,0.00017]
                                           
                                           
        self._OdomData2.header.frame_id = self._OdomData1.header.frame_id
        self._OdomData2.child_frame_id = self._OdomData1.child_frame_id
        self._OdomData2.pose.covariance = self._OdomData1.pose.covariance
        self._OdomData2.twist.covariance = self._OdomData1.twist.covariance
        
        self._seq = 0