def pose_update_cb(pose_data):
"""
Publishes odometry information
:param pose_data: geometry_msgs/PoseWithCovarianceStamped, current pose data from hector_mapping package.
:return:
"""
# Broadcast transform base_link -> odom
odom_broadcaster.sendTransform(
(pose_data.pose.pose.position.x, pose_data.pose.pose.position.y,
pose_data.pose.pose.position.z),
(pose_data.pose.pose.orientation.x, pose_data.pose.pose.orientation.y,
pose_data.pose.pose.orientation.z, pose_data.pose.pose.orientation.w),
pose_data.header.stamp, "base_link", "odom")
# Next, we'll publish the odometry message over ROS
odom = Odometry()
odom.header = pose_data.header
odom.header.frame_id = "odom"
# Set the position
odom.pose = pose_data.pose
# Set the velocity
odom.child_frame_id = "base_link"
odom.twist.twist = Twist(Vector3(vel_x, 0, 0), Vector3(0, 0, vel_yaw))
# Publish the message
odom_pub.publish(odom)
def pub_ekf_odom(self, msg):
odom = Odometry()
odom.header = msg.header
odom.child_frame_id = 'angelbot_base'
odom.pose = msg.pose
self.ekf_pub.publish(odom)
def odom_callback(self, msg):
odom_rotated = Odometry()
odom = msg
pose = odom.pose
twist = odom.twist
#tranform twist into odom frame
linear_twist = twist.twist.linear
angular_twist = twist.twist.angular
twist_rotated = TwistWithCovariance()
transformed_linear = self.transform_meas_to_world(
tl(linear_twist), odom.child_frame_id, odom.header.frame_id,
rospy.Time(0), False)
transformed_angular = self.transform_meas_to_world(
tl(angular_twist), odom.child_frame_id, odom.header.frame_id,
rospy.Time(0), False)
twist_rotated.twist.linear = tm(transformed_linear, Vector3)
twist_rotated.twist.angular = tm(transformed_angular, Vector3)
twist_rotated.covariance = twist.covariance #may need to be transformed
odom_rotated.header = odom.header
odom_rotated.child_frame_id = odom.child_frame_id
odom_rotated.pose = pose
odom_rotated.twist = twist_rotated
self.rotated_twist_pub.publish(odom_rotated)
def publish_odometry(self, odom_combined):
"""
use current pose, along with delta from last pose to publish
the current Odometry on the /odom topic
"""
if not self.last_time:
# set up initial times and pose
rospy.loginfo("Setting up initial position")
self.last_time, self.last_x, y, self.last_theta = self.current_pose(odom_combined)
return
# publish to the /odom topic
odom = Odometry()
odom.header.stamp = rospy.Time.now()
odom.header.frame_id = "/base_link"
odom.pose = odom_combined.pose
current_time, x, y, theta = self.current_pose(odom_combined)
dt = current_time - self.last_time
dt = dt.to_sec()
d_x = x - self.last_x
d_theta = theta - self.last_theta
odom.twist.twist = Twist(Vector3(d_x/dt, 0, 0), Vector3(0, 0, d_theta/dt))
self.odom_publisher.publish(odom)
self.last_time, self.last_x, self.last_theta = current_time, x, theta
def pub_odom(self, msg):
odom = Odometry()
odom.header = msg.header
odom.child_frame_id = self.base_frame
odom.pose = msg.pose
self.odom_pub.publish(odom)
def svo_callback(self, data):
# converts to nav_msg odometry type msg and publish
odom = Odometry()
odom.pose = data.pose
odom.twist = self.twist
self.pub_vo.publish(odom)
def pub_ekf_odom(self, msg):
odom = Odometry()
odom.header = msg.header
odom.child_frame_id = 'base_footprint'
odom.pose = msg.pose
self.ekf_pub.publish(odom)
def setupOdometry(self, data):
odom = Odometry()
odom.header.stamp = rospy.Time.now()
odom.header.frame_id = "odom"
odom.pose = self.packagePose(data)
odom.twist = self.packageTwist(data)
return odom
def __update_odometry(self, linear_offset, angular_offset, tf_delay): self.__heading = (self.__heading + angular_offset) % 360 q = tf.transformations.quaternion_from_euler(0, 0, math.radians(self.__heading)) self.__pose.position.x += math.cos(math.radians(self.__heading)) * self.__distance_per_cycle * self.__twist.linear.x self.__pose.position.y += math.sin(math.radians(self.__heading)) * self.__distance_per_cycle * self.__twist.linear.x self.__pose.position.z = 0.33 self.__pose.orientation = Quaternion(q[0], q[1], q[2], q[3]) now = rospy.Time.now() + rospy.Duration(tf_delay) self.__tfb.sendTransform( (self.__pose.position.x, self.__pose.position.y, self.__pose.position.z), q, now, 'base_link', 'odom') o = Odometry() o.header.stamp = now o.header.frame_id = 'odom' o.child_frame_id = 'base_link' o.pose = PoseWithCovariance(self.__pose, None) o.twist = TwistWithCovariance() o.twist.twist.linear.x = self.__distance_per_second * self.__twist.linear.x o.twist.twist.angular.z = math.radians(self.__rotation_per_second) * self.__twist.angular.z
def odom_callback(data):
odom = Odometry()
odom.header.frame_id = odom_frame
odom.child_frame_id = base_frame
odom.header.stamp = rospy.Time.now()
odom.pose = data.pose
odom.pose.pose.position.x = odom.pose.pose.position.x - 2.5
odom.pose.pose.position.y = odom.pose.pose.position.y + 7.0
odom.twist = data.twist
tf = TransformStamped(header = Header(
frame_id = odom.header.frame_id,
stamp = odom.header.stamp),
child_frame_id = odom.child_frame_id,
transform = Transform(
translation = odom.pose.pose.position,
rotation = odom.pose.pose.orientation))
# visualize footprint everytime odom changes
footprint_visualizer()
odom_pub.publish(odom)
tf_pub.sendTransform(tf)
def pub_ekf_odom(self, msg):
odom = Odometry()
odom.header = msg.header
odom.child_frame_id = 'base_footprint'
odom.pose = msg.pose
self.ekf_pub.publish(odom)
def publishPose():
covariance_ = [
0.25, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.25, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0
]
x = 0
y = 0
z = 0
qx = 0
qy = 0
qz = 0
qw = 1
time_now = rospy.Time.now()
id_ = '/map'
poseWCS = PoseWithCovarianceStamped()
poseWCS.header.stamp = time_now
poseWCS.header.frame_id = id_
poseWCS.pose.pose = Pose(Point(x, y, z), Quaternion(qx, qy, qz, qw))
poseWCS.pose.covariance = covariance_
initialpose_poseWCS_publisher.publish(poseWCS)
poseWC = PoseWithCovariance()
poseWC.pose = Pose(Point(x, y, z), Quaternion(qx, qy, qz, qw))
poseWC.covariance = covariance_
twistWC = TwistWithCovariance()
twistWC.twist = Twist(Vector3(0.0, 0.0, 0.0), Vector3(0.0, 0.0, 0.0))
twistWC.covariance = covariance_
odom = Odometry()
odom.header.stamp = time_now
odom.header.frame_id = id_
odom.pose = poseWC
odom.twist = twistWC
fakelocalisation_poseWCS_publisher.publish(odom)
def main(self):
sub = rospy.Subscriber('robot_pose_ekf/odom_combined',
PoseWithCovarianceStamped, self.pose_callback)
sub = rospy.Subscriber('odom', Odometry, self.twist_callback)
# Rate at which the program runs in (hz) how many times per second
hz_rate = 15.0
current_time = rospy.Time.now()
r = rospy.Rate(hz_rate)
while not rospy.is_shutdown():
current_time = rospy.Time.now()
# next, we'll publish the odometry message over ROS
odom = Odometry()
odom.header.stamp = current_time
odom.header.frame_id = "fake_odom"
if self.pose_data != 0:
odom.pose = self.pose_data
odom.twist = self.twist_data
odom.child_frame_id = "base_link"
# publish the message
self.localization_pub.publish(odom)
r.sleep()
def GetOdomFromState(state, spot_wrapper, use_vision=True):
"""Maps odometry data from robot state proto to ROS Odometry message
Args:
data: Robot State proto
spot_wrapper: A SpotWrapper object
Returns:
Odometry message
"""
odom_msg = Odometry()
local_time = spot_wrapper.robotToLocalTime(
state.kinematic_state.acquisition_timestamp)
odom_msg.header.stamp = rospy.Time(local_time.seconds, local_time.nanos)
if use_vision == True:
odom_msg.header.frame_id = 'vision'
tform_body = get_vision_tform_body(
state.kinematic_state.transforms_snapshot)
else:
odom_msg.header.frame_id = 'odom'
tform_body = get_odom_tform_body(
state.kinematic_state.transforms_snapshot)
odom_msg.child_frame_id = 'body'
pose_odom_msg = PoseWithCovariance()
pose_odom_msg.pose.position.x = tform_body.position.x
pose_odom_msg.pose.position.y = tform_body.position.y
pose_odom_msg.pose.position.z = tform_body.position.z
pose_odom_msg.pose.orientation.x = tform_body.rotation.x
pose_odom_msg.pose.orientation.y = tform_body.rotation.y
pose_odom_msg.pose.orientation.z = tform_body.rotation.z
pose_odom_msg.pose.orientation.w = tform_body.rotation.w
odom_msg.pose = pose_odom_msg
twist_odom_msg = GetOdomTwistFromState(state, spot_wrapper).twist
odom_msg.twist = twist_odom_msg
return odom_msg
def __update_odometry(self, linear_offset, angular_offset, tf_delay):
self.__heading = (self.__heading + angular_offset) % 360
q = tf.transformations.quaternion_from_euler(
0, 0, math.radians(self.__heading))
self.__pose.position.x += math.cos(
math.radians(self.__heading
)) * self.__distance_per_cycle * self.__twist.linear.x
self.__pose.position.y += math.sin(
math.radians(self.__heading
)) * self.__distance_per_cycle * self.__twist.linear.x
self.__pose.position.z = 0.33
self.__pose.orientation = Quaternion(q[0], q[1], q[2], q[3])
now = rospy.Time.now() + rospy.Duration(tf_delay)
self.__tfb.sendTransform(
(self.__pose.position.x, self.__pose.position.y,
self.__pose.position.z), q, now, 'base_link', 'odom')
o = Odometry()
o.header.stamp = now
o.header.frame_id = 'odom'
o.child_frame_id = 'base_link'
o.pose = PoseWithCovariance(self.__pose, None)
o.twist = TwistWithCovariance()
o.twist.twist.linear.x = self.__distance_per_second * self.__twist.linear.x
o.twist.twist.angular.z = math.radians(
self.__rotation_per_second) * self.__twist.angular.z
def state_2_odom(state_vec, covariance, frame="odom"):
out = Odometry()
out.header.stamp = rospy.Time.now()
out.header.frame_id = frame
out.pose = state_to_pose_with_cov(state_vec, covariance)
out.twist = state_to_twist_with_cov(state_vec, covariance)
return out
def callback(data):
print data
odom = Odometry()
odom.pose = data.pose
odom.header.stamp = rospy.Time.now()
odom.header.frame_id = "/odom"
global pub
pub.publish(odom)
def callback(data):
print data
odom = Odometry()
odom.pose = data.pose
odom.header.stamp = rospy.Time.now()
odom.header.frame_id = "/odom"
global pub
pub.publish(odom)
def pub_ekf_odom(self, msg):
odom = Odometry()
odom.header = msg.header
odom.header.frame_id = '/odom'
odom.child_frame_id = 'base_link'
odom.pose = msg.pose
self.ekf_pub.publish(odom)
def publish(self):
trans = self.setupTransform(None)
odom = Odometry()
odom.header.stamp = rospy.Time.now()
odom.header.frame_id = "odom"
odom.pose = PoseWithCovariance()
odom.twist = TwistWithCovariance()
self.tf_pub.sendTransform(trans)
self.pub.publish(odom)
def callback(self, data):
lat = data.latitude
lon = data.longitude
alt = data.altitude
e, n, self.zone_number, self.zone_letter = utm.from_latlon(lat, lon)
if not self.start_pos_was_set:
self.start_e = e
self.start_n = n
"""
if self.offset:
self.start_e -= self.offset.translation.x
self.start_n -= self.offset.translation.y
"""
if self.offset:
self.start_e -= 0.4
self.start_n -= 0.4
self.start_pos_was_set = True
self.start_alt = data.altitude
position = Point()
position.x = e - self.start_e
position.y = n - self.start_n
position.z = 0.0
erel = e - self.start_e # relative position to start position in meters
nrel = n - self.start_n
alt_rel = alt - self.start_alt
# empty as the gps data doesnt include orientation
orientation = Quaternion()
orientation.w = 1.0
mypose = Pose()
mypose.orientation = orientation
mypose.position = position
if self.fix_covariance:
covariance = np.array([1.5, 0, 0, 0, 1.5, 0, 0, 0, 3])
else:
covariance = data.position_covariance
covariance = np.reshape(covariance, (3, 3))
cov6x6 = np.zeros((6, 6))
cov6x6[:3, :3] = covariance
cov6x6_flat = np.reshape(cov6x6, (36, )).astype(np.float64)
pose_with_cov = PoseWithCovariance()
pose_with_cov.covariance = cov6x6_flat
pose_with_cov.pose = mypose
odom = Odometry()
odom.pose = pose_with_cov
odom.header = data.header
odom.header.stamp = data.header.stamp
odom.header.stamp = rospy.Time.now()
odom.header.frame_id = "map"
# odom.twist stays empty as gps doesn't have this data
self.publisher.publish(odom)
def pose1_callback(self, data):
pose = data.pose.pose
self.bf1_pose = pose
self.bf1_vel = data.twist.twist
# publish a /odom_pose topic
odom = Odometry()
odom.pose = data.pose
odom.twist = self.vel_cmd1.twist
self.vehicle_odom_pub.publish(odom)
def callback(
msg
): #define call back function, when there is data published on /odometry/odom the function will be called and processed data will be published on /mavros/odometry/out
odom = Odometry()
odom.header.stamp = rospy.Time.now()
odom.header.frame_id = "odom_ned"
odom.child_frame_id = "base_link_frd"
odom.pose = msg.pose
odom.twist = msg.twist
publisher.publish(odom)
def odom_received(odom):
global target_child_frame
global new_odom_frame
global publish_odometry_transforms
global drift_vector
global tfBroadcaster
global tfBuffer
global odom_publisher
global first_stamp
ros_transform = tfBuffer.lookup_transform(target_child_frame,
odom.child_frame_id,
odom.header.stamp)
transform = ros_transform_to_matrix(ros_transform)
pose = ros_pose_to_matrix(odom.pose.pose)
twist = ros_twist_to_matrix(odom.twist.twist)
new_pose = transform @ pose @ np.linalg.inv(transform)
new_position = new_pose[:3, 3]
new_orientation = mat2quat(new_pose[:3, :3])
new_twist = transform @ twist @ np.linalg.inv(transform)
new_linear = new_twist[:3, 3]
new_angular = skew_to_vector(new_twist[:3, :3])
if first_stamp == rospy.Time(0):
first_stamp = odom.header.stamp
new_position += drift_vector * (odom.header.stamp - first_stamp).to_sec()
new_linear += drift_vector
if odom_publisher is not None:
new_odom = Odometry(header=deepcopy(odom.header))
new_odom.header.frame_id = new_odom_frame
new_odom.child_frame_id = target_child_frame
new_odom.pose = PoseWithCovariance(pose=Pose(
Point(new_position[0], new_position[1], new_position[2]),
Quaternion(new_orientation[1], new_orientation[2],
new_orientation[3], new_orientation[0])))
new_odom.twist = TwistWithCovariance(twist=Twist(
Vector3(new_linear[0], new_linear[1], new_linear[2]),
Vector3(new_angular[0], new_angular[1], new_angular[2])))
odom_publisher.publish(new_odom)
if publish_odometry_transforms:
transform = TransformStamped(header=deepcopy(odom.header))
transform.header.frame_id = new_odom_frame
transform.child_frame_id = target_child_frame
transform.transform = Transform(
Vector3(new_position[0], new_position[1], new_position[2]),
Quaternion(new_orientation[1], new_orientation[2],
new_orientation[3], new_orientation[0]))
tfBroadcaster.sendTransform(transform)
def pose1_callback(self, data):
pose = data.pose.pose
self.bf1_pose = pose
self.bf1_vel = data.twist.twist
T_bf1 = tf.transformations.quaternion_matrix([
pose.orientation.x, pose.orientation.y, pose.orientation.z,
pose.orientation.w
])
T_bf1[:3, 3] = np.array(
[pose.position.x, pose.position.y, pose.position.z])
# calculate lidar pose
T_lidar = np.dot(
tf.transformations.rotation_matrix(np.pi / 2, direction=(0, 0, 1)),
tf.transformations.rotation_matrix(np.pi / 2, direction=(1, 0, 0)))
#T_lidar = tf.transformations.euler_matrix(-np.pi/2, -np.pi/2, 0)
# T_lidar[0,3] += 0.2
T_lidar[1, 3] += 0.08
T_lidar[2, 3] += -0.15
T_lidar = np.dot(T_bf1, T_lidar)
# # make lidar spin
T_lidar = np.dot(
T_lidar,
tf.transformations.rotation_matrix(10. * self.lidar_counter,
direction=(0, 1, 0)))
self.lidar_counter += 1
if self.lidar_counter > 2000:
self.lidar_counter = 0
quater_lidar = tf.transformations.quaternion_from_matrix(T_lidar)
# make lidar follow
lidar_odom = Odometry()
lidar_odom.pose.pose.position.x = T_lidar[0, 3]
lidar_odom.pose.pose.position.y = T_lidar[1, 3]
lidar_odom.pose.pose.position.z = T_lidar[2, 3]
lidar_odom.pose.pose.orientation.x = quater_lidar[0]
lidar_odom.pose.pose.orientation.y = quater_lidar[1]
lidar_odom.pose.pose.orientation.z = quater_lidar[2]
lidar_odom.pose.pose.orientation.w = quater_lidar[3]
self.lidar_pub.publish(lidar_odom)
# publish a /odom_pose topic
odom = Odometry()
odom.pose = data.pose
odom.twist = self.vel_cmd1.twist
self.vehicle_odom_pub.publish(odom)
def get_origin_odom_msg():
""" Get ros odometry message at origin.
Returns:
Odometry: ROR odometry message with
position = (0, 0, 0) and
quaternion = (0, 0, 0, 1)
"""
msg = Odometry()
pose = PoseWithCovariance()
pose.pose.orientation.w = 1
msg.pose = pose
return msg
def publish_sensors(self, msg):
""" publishes noisy position values for each robot """
pwc = PoseWithCovariance()
twc = TwistWithCovariance()
auv = "akon"
# X Meas
rospy.loginfo_once("Normal Error on X")
sigma = rospy.get_param("kalman/measurements/x_sigma")
dot_sigma = rospy.get_param("kalman/measurements/x_dot_sigma")
pwc.pose.position.x = np.random.normal(
self.auvs[auv][ODOM_INDEX].pose.pose.position.x, sigma)
pwc.covariance[0] = sigma**2
twc.twist.linear.x = np.random.normal(
self.auvs[auv][ODOM_INDEX].twist.twist.linear.x, dot_sigma)
twc.covariance[0] = dot_sigma**2
# Y Meas
rospy.loginfo_once("Normal Error on Y")
sigma = rospy.get_param("kalman/measurements/y_sigma")
dot_sigma = rospy.get_param("kalman/measurements/y_dot_sigma")
pwc.pose.position.y = np.random.normal(
self.auvs[auv][ODOM_INDEX].pose.pose.position.y, sigma)
pwc.covariance[7] = sigma**2
twc.twist.linear.y = np.random.normal(
self.auvs[auv][ODOM_INDEX].twist.twist.linear.y, dot_sigma)
twc.covariance[7] = dot_sigma**2
# Z Meas
rospy.loginfo_once("Normal Error on Z")
sigma = rospy.get_param("kalman/measurements/z_sigma")
dot_sigma = rospy.get_param("kalman/measurements/z_dot_sigma")
pwc.pose.position.z = np.random.normal(
self.auvs[auv][ODOM_INDEX].pose.pose.position.z, sigma)
pwc.covariance[14] = sigma**2
twc.twist.linear.z = np.random.normal(
self.auvs[auv][ODOM_INDEX].twist.twist.linear.z, dot_sigma)
twc.covariance[14] = dot_sigma**2
pwc.pose.orientation = self.auvs[auv][ODOM_INDEX].pose.pose.orientation
odom_msg = Odometry()
odom_msg.header.seq = self.seq
odom_msg.header.stamp = rospy.Time.now()
odom_msg.header.frame_id = "base_link"
odom_msg.child_frame_id = odom_msg.header.frame_id
odom_msg.pose = pwc
odom_msg.twist = twc
self.odom_sensor_pub.publish(odom_msg)
self.seq += 1
def process_mocap_data(self):
r = rospy.Rate(rate)
while not rospy.is_shutdown():
if self.tf.frameExists(self.frame) and self.tf.frameExists("/world"):
t = self.tf.getLatestCommonTime(self.frame, "/world")
z, q = self.tf.lookupTransform("/world", self.frame, t)
#yawtest = tf.transformations.euler_from_quaternion(q)[2]
self.kr3.step(z)
#yaw = self.getYaw(q)
#print yaw, yawtest
#print roll_test pitch_test yawtest
odom = Odometry()
odom.header.stamp = rospy.Time.now()
odom.header.frame_id = "/world"
odom.child_frame_id = "/" + self.name + "/base_link"
#quat = tf.transformations.quaternion_from_euler(0.0, 0.0, yaw)
posecov = PoseWithCovariance()
pose = Pose()
pose.position.x = self.kr3.pos[0]
pose.position.y = self.kr3.pos[1]
pose.position.z = self.kr3.pos[2]
pose.orientation.x = q[0]
pose.orientation.y = q[1]
pose.orientation.z = q[2]
pose.orientation.w = q[3]
posecov.pose = pose
posecov.covariance = [0] * 36
odom.pose = posecov
twistcov = TwistWithCovariance()
twist = Twist()
twist.linear.x = self.kr3.vel[0]
twist.linear.y = self.kr3.vel[1]
twist.linear.z = self.kr3.vel[2]
twist.angular.x = 0.0
twist.angular.y = 0.0
twist.angular.z = 0.0
twistcov.twist = twist
twistcov.covariance = [0] * 36
odom.twist = twistcov
self.odom_pub.publish(odom)
poseStamped = PoseStamped()
poseStamped.pose = pose
poseStamped.header = odom.header
poseStamped.header.frame_id = "/" + self.name + "/base_link"
self.pose_pub.publish(poseStamped)
r.sleep()
def convert(data): global prevTime; global prevPose; global isInitIter# = True; currentTime = rospy.Time.now(); if (isInitIter): prevTime = currentTime; dt = 99999999999999999; else: dt = currentTime.to_sec() - prevTime.to_sec(); #Get delta time. if (isInitIter): prevPose = data.pose; #Note first pose is a PoseWithCovariance. isInitIter = False; currentPose = PoseWithCovariance(); currentPose = data.pose; #Note first pose is a PoseWithCovariance. currentTwist = TwistWithCovariance(); #Calculate velocities. currentTwist.twist.linear.x = (currentPose.pose.position.x - prevPose.pose.position.x)/dt; currentTwist.twist.linear.y = (currentPose.pose.position.y - prevPose.pose.position.y)/dt; currentTwist.twist.linear.z = (currentPose.pose.position.z - prevPose.pose.position.z)/dt; currentTwist.twist.angular.x = (currentPose.pose.orientation.x - prevPose.pose.orientation.x)/dt; currentTwist.twist.angular.y = (currentPose.pose.orientation.y - prevPose.pose.orientation.y)/dt; currentTwist.twist.angular.z = (currentPose.pose.orientation.z - prevPose.pose.orientation.z)/dt; currentTwist.covariance = data.pose.covariance; # Make the message to publish: odomMessage = Odometry(); odomMessage.header = data.header; #odomMessage.header.frame_id = "something custom?" odomMessage.child_frame_id = "base_link"; odomMessage.pose = currentPose; odomMessage.twist = currentTwist; # publish the message p.publish(odomMessage); prevPose = currentPose;
def odom_callback(data):
odom = Odometry()
odom.header.stamp = rospy.Time.now()
odom.header.frame_id = 'odom' # 'map'
odom.child_frame_id = 'base_link' # 'odom'
odom.pose = data.pose
odom.pose.pose.position.y = odom.pose.pose.position.y + 5.0
odom.twist = data.twist
tf = TransformStamped(header=Header(frame_id=odom.header.frame_id,
stamp=odom.header.stamp),
child_frame_id=odom.child_frame_id,
transform=Transform(
translation=odom.pose.pose.position,
rotation=odom.pose.pose.orientation))
odom_pub.publish(odom)
tf_pub.sendTransform(tf)
def publish_wheel_odom(self):
odom_msg = Odometry()
odom_msg.header = Header()
odom_msg.header.stamp = rospy.Time.now()
odom_msg.header.frame_id = "odom"
odom_msg.child_frame_id = "scout_1_tf/base_footprint"
odom_msg.pose = PoseWithCovariance()
odom_msg.pose.pose = Pose()
odom_msg.pose.pose.position.x = self.wheel_odom[0]
odom_msg.pose.pose.position.y = self.wheel_odom[1]
#odom_msg.pose.pose.position.z = self.wheel_odom_z
odom_msg.twist = TwistWithCovariance()
odom_msg.twist.twist = Twist()
self.wheel_odom_pub.publish(odom_msg)
def talker():
pub = rospy.Publisher("/mapping_planning/waypoints", Path, queue_size=10)
pub2 = rospy.Publisher("/localization/pose1", Odometry, queue_size=10)
rospy.init_node('talker', anonymous=True)
rate = rospy.Rate(10) # 10hz
print("Publishing empty waypoints")
while not rospy.is_shutdown():
#Create waypoint msg
posestamped = PoseStamped()
posestamped.header.stamp = rospy.Time.now()
waypoints = Path()
waypoints.header.stamp = rospy.Time.now()
waypoints.poses.append(posestamped)
waypoints.poses.append(posestamped)
#Create odometry msg
odometry_msg = Odometry()
odometry_msg.header.stamp = rospy.Time.now()
odometry_msg.header.frame_id = "ego_vehicle"
odometry_msg.child_frame_id = "map"
pose1 = PoseWithCovariance()
pose2 = Pose()
point = Point()
point.x = 5
point.y = -45
point.z = 0.01
quat = Quaternion()
quat.x = 0
quat.y = 0
quat.z = 0.7071
quat.w = 0.7071
pose2.position = point
pose2.orientation = quat
pose1.pose = pose2
odometry_msg.pose = pose1
#Publish msgs
pub.publish(waypoints)
pub2.publish(odometry_msg)
rate.sleep()
def cbfake(self, msg):
self.threadC.acquire()
self.world.pose.pose.position.x = msg.pose.pose.position.x
self.world.pose.pose.position.y = msg.pose.pose.position.y
self.world.pose.pose.position.z = msg.pose.pose.position.z
self.world.pose.pose.orientation.x = msg.pose.pose.orientation.x
self.world.pose.pose.orientation.y = msg.pose.pose.orientation.y
self.world.pose.pose.orientation.z = msg.pose.pose.orientation.z
self.world.pose.pose.orientation.w = msg.pose.pose.orientation.w
tmp1 = Odometry()
tmp1.child_frame_id = "base_link"
tmp1.pose = msg.pose
tmp1.twist = msg.twist
tmp1.header.frame_id = "odom"
self.br.sendTransform((self.world.pose.pose.position.x,self.world.pose.pose.position.y,self.world.pose.pose.position.z)\
,(self.world.pose.pose.orientation.x,self.world.pose.pose.orientation.y,self.world.pose.pose.orientation.z,self.world.pose.pose.orientation.w),rospy.Time.now(),"base_link","odom")
self.pub.publish(tmp1)
self.threadC.release()
def callback(pub, data):
odom = Odometry()
pose = PoseWithCovariance()
twist = TwistWithCovariance()
#no shift in relation to boat position
pose.pose.position.x = 0
pose.pose.position.y = 0
pose.pose.position.z = 0
#no rotation in relation to boat position
pose.pose.orientation.x = 0
pose.pose.orientation.y = 0
pose.pose.orientation.z = 0
pose.pose.orientation.w = 1
twist.twist = data
odom.pose = pose
odom.twist = twist
odom.header.frame_id = "base_link"
odom.child_frame_id = "base_link"
pub.publish(odom)
def callback(data):
odom = Odometry()
odom.header = data.header
odom.child_frame_id = child_frame_id
odom.pose = data.pose
pub.publish(odom)
