Python geometry_msgs.msg.PoseWithCovarianceStamped() Examples
The following are 7
code examples of geometry_msgs.msg.PoseWithCovarianceStamped().
You can vote up the ones you like or vote down the ones you don't like,
and go to the original project or source file by following the links above each example.
You may also want to check out all available functions/classes of the module
geometry_msgs.msg
, or try the search function
.
.
Example #1
| Source File: task_generator.py From drl_local_planner_ros_stable_baselines with BSD 3-Clause "New" or "Revised" License | 6 votes |
|
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
Example #2
| Source File: trajectory_planner.py From TrajectoryPlanner with MIT License | 6 votes |
|
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
Example #3
| Source File: helpers.py From kinova-movo with BSD 3-Clause "New" or "Revised" License | 6 votes |
|
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
Example #4
| Source File: get_angular_combined.py From dashgo with Apache License 2.0 | 5 votes |
|
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))
Example #5
| Source File: odom_ekf.py From dashgo with Apache License 2.0 | 5 votes |
|
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")
Example #6
| Source File: move_base.py From kinova-movo with BSD 3-Clause "New" or "Revised" License | 5 votes |
|
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
Example #7
| Source File: movo_data_classes.py From kinova-movo with BSD 3-Clause "New" or "Revised" License | 4 votes |
|
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
