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 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 __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 __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 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 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 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 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 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 update_pose_and_publish(self, pose_stamped_msg: PoseStamped):
if self.last_odom is None:
rospy.loginfo("waiting for odom input")
return
fused_odom = Odometry()
fused_odom.header = pose_stamped_msg.header
fused_odom.child_frame_id = self.last_odom.child_frame_id
fused_odom.twist = self.last_odom.twist
fused_odom.pose.pose = pose_stamped_msg.pose
self.odom_pub.publish(fused_odom)
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 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 pub_odom(self, odometry_msg):
odometry = Odometry()
odometry.header = odometry_msg.header
odometry.child_frame_id = "base_link"
odometry.pose.pose = odometry_msg.pose.pose
covariance = [0] * 36
covariance[0] = 0.1
covariance[1] = 0.1
covariance[5] = 0.00001
odometry.pose.covariance = covariance
odometry.twist = odometry_msg.twist
self.pub.publish(odometry)
def publish_pose_stamped(self):
if self.base_odom == None:
return
try:
(trans,rot) = self.listener.lookupTransform(self.map_frame, self.base_odom.child_frame_id, rospy.Time(0)) # current map->base transform
except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
return
pub_msg = Odometry()
pub_msg.pose.pose.position.x = trans[0]
pub_msg.pose.pose.position.y = trans[1]
pub_msg.pose.pose.position.z = trans[2]
pub_msg.pose.pose.orientation.x = rot[0]
pub_msg.pose.pose.orientation.y = rot[1]
pub_msg.pose.pose.orientation.z = rot[2]
pub_msg.pose.pose.orientation.w = rot[3]
pub_msg.header.stamp = rospy.Time.now()
pub_msg.header.frame_id = self.map_frame
pub_msg.child_frame_id = self.base_odom.child_frame_id
# calculate covariance
# use covariance of base_odom.pose and only transform it to new coords from tf
# TODO: calc covariance in correct way, but what is that ...?
base_odom_homogeneous_matrix = self.make_homogeneous_matrix([self.base_odom.pose.pose.position.x, self.base_odom.pose.pose.position.y, self.base_odom.pose.pose.position.z],
[self.base_odom.pose.pose.orientation.x, self.base_odom.pose.pose.orientation.y, self.base_odom.pose.pose.orientation.z, self.base_odom.pose.pose.orientation.w])
new_odom_homogeneous_matrix = self.make_homogeneous_matrix(trans, rot)
base_to_new_transform = numpy.dot(base_odom_homogeneous_matrix, numpy.linalg.inv(new_odom_homogeneous_matrix))
rotation_matrix = base_to_new_transform[:3, :3]
original_pose_cov_matrix = numpy.matrix(self.base_odom.pose.covariance).reshape(6, 6)
pose_cov_matrix = numpy.zeros((6, 6))
pose_cov_matrix[:3, :3] = (rotation_matrix.T).dot(original_pose_cov_matrix[:3, :3].dot(rotation_matrix))
pose_cov_matrix[3:6, 3:6] = (rotation_matrix.T).dot(original_pose_cov_matrix[3:6, 3:6].dot(rotation_matrix))
pub_msg.pose.covariance = numpy.array(pose_cov_matrix).reshape(-1,).tolist()
# twist is local and transform same as covariance
new_velocity = numpy.dot(rotation_matrix, numpy.array([[self.base_odom.twist.twist.linear.x],
[self.base_odom.twist.twist.linear.y],
[self.base_odom.twist.twist.linear.z]]))
new_omega = numpy.dot(rotation_matrix, numpy.array([[self.base_odom.twist.twist.angular.x],
[self.base_odom.twist.twist.angular.y],
[self.base_odom.twist.twist.angular.z]]))
original_twist_cov_matrix = numpy.matrix(self.base_odom.twist.covariance).reshape(6, 6)
twist_cov_matrix = numpy.zeros((6, 6))
twist_cov_matrix[:3, :3] = (rotation_matrix.T).dot(original_twist_cov_matrix[:3, :3].dot(rotation_matrix))
twist_cov_matrix[3:6, 3:6] = (rotation_matrix.T).dot(original_twist_cov_matrix[3:6, 3:6].dot(rotation_matrix))
pub_msg.twist = TwistWithCovariance(Twist(Vector3(*new_velocity[:, 0]), Vector3(*new_omega[:, 0])), twist_cov_matrix.reshape(-1,).tolist())
self.pub.publish(pub_msg)
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 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 receive_encoders(self, msg):
header = Header()
header.stamp = rospy.Time.now()
header.frame_id = 'base_link'
point_msg = Point(0, 0, 0)
orientation_quat = R.from_euler('xyz', [0, 0, 0]).as_quat()
pose_cov = np.ravel(np.eye(6) * 0.0)
quat_msg = Quaternion(orientation_quat[0], orientation_quat[1],
orientation_quat[2], orientation_quat[3])
pose_with_cov = PoseWithCovariance()
pose_with_cov.pose = Pose(point_msg, quat_msg)
pose_with_cov.covariance = pose_cov
x_dot = (((msg.left * math.pi / 30 * self.wheel_radius) +
(msg.right * math.pi / 30 * self.wheel_radius)) / 2
) # * math.cos(self.orientation[-1])
y_dot = 0 #(((self.port_encoder * math.pi / 30 * 0.2286) + (self.starboard_encoder * math.pi / 30 * 0.2286)) / 2) * math.sin(self.orientation[-1])
theta_dot = (
(msg.right * math.pi / 30 * self.wheel_radius) -
(msg.left * math.pi / 30 * self.wheel_radius)) / self.robot_width
linear_twist = Vector3(x_dot, y_dot, 0)
angular_twist = Vector3(0, 0, theta_dot)
twist_cov = np.diag([0.002, 0.02, 0, 0, 0, 0.001]).flatten()
twist_with_cov = TwistWithCovariance()
twist_with_cov.twist = Twist(linear_twist, angular_twist)
twist_with_cov.covariance = twist_cov
stamped_msg = Odometry()
stamped_msg.header = header
stamped_msg.child_frame_id = 'base_link'
stamped_msg.pose = pose_with_cov
stamped_msg.twist = twist_with_cov
try:
self.wheel_pub.publish(stamped_msg)
except rospy.ROSInterruptException as e:
rospy.logerr(e.getMessage())
def publishOdometry(self):
header = Header()
header.stamp = rospy.Time.now()
header.frame_id = 'map'
msg = Odometry()
msg.header = header
msg.child_frame_id = "base_link"
pose = Pose()
pose.position = Point(self.robot.state[0, 0], self.robot.state[1, 0],
0)
quat = R.from_euler('z', self.robot.state[2, 0]).as_quat()
pose.orientation = Quaternion(quat[0], quat[1], quat[2], quat[3])
msg.pose = PoseWithCovariance(pose=pose)
twist = Twist()
twist.linear.x = self.robot.state_dot[0, 0]
twist.angular.z = self.robot.state_dot[2, 0]
msg.twist = TwistWithCovariance(twist=twist)
self.odometryPublisher.publish(msg)
def call_services():
time_now = rospy.Time.now()
poseWCS = PoseWithCovarianceStamped()
poseWCS.header.stamp = time_now
poseWCS.header.frame_id = '/map'
poseWCS.pose.pose = Pose(Point(-1, 4, 0.0), Quaternion(0.0, 0.0, 0.0, 1.0))
poseWCS.pose.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, 0.06853891945200942
]
odom = Odometry()
odom.header.stamp = time_now
odom.header.frame_id = '/map'
poseWC = PoseWithCovariance()
poseWC.pose = Pose(Point(-1, 4, 0.0), Quaternion(0.0, 0.0, 0.0, 1.0))
poseWC.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, 0.06853891945200942
]
odom.pose = poseWC
twistWC = TwistWithCovariance()
twistWC.twist = Twist(Vector3(0.0, 0.0, 0.0), Vector3(0.0, 0.0, 0.0))
twistWC.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, 0.06853891945200942
]
odom.twist = twistWC
rospy.wait_for_service('/Pioneer2_vc_initialpose')
try:
initialpose_proxy = rospy.ServiceProxy('/Pioneer2_vc_initialpose',
InitialPoseToRobot)
resp = initialpose_proxy(poseWCS)
except rospy.ServiceException, e:
print '/Pioneer2_vc_initialpose service call failed: %s' % e
def __init__(self):
self.set_init_pose = 0
self.init_pose = None
self.curr_pose = None
self.init_twist = None
self.curr_twist = None
self.rover_name = rospy.get_param("rover_name", "scout_1")
rospy.Subscriber("/gazebo/model_states", ModelStates, callback=self.model_states_callback)
self.gt_pub = rospy.Publisher("/debugging/gt_odom", Odometry, queue_size=5)
self.init_pose_pub = rospy.Publisher("/debugging/init_pose", Pose, queue_size=5)
rospy.init_node('rover_localization_gt')
rate = rospy.Rate(30) # 30hz
while not rospy.is_shutdown():
if self.curr_pose is not None and self.curr_twist is not None:
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 = self.rover_name
odom_msg.pose = PoseWithCovariance()
odom_msg.pose.pose = self.curr_pose
odom_msg.twist = TwistWithCovariance()
odom_msg.twist.twist = self.curr_twist
self.gt_pub.publish(odom_msg)
if self.init_pose is not None and self.init_twist is not None:
self.init_pose_pub.publish(self.init_pose)
rate.sleep()
def publish_pose(self, pose, twist, child_frame='base_link'):
orientation = [
pose.pose.orientation.x, pose.pose.orientation.y,
pose.pose.orientation.z, pose.pose.orientation.w
]
position = [
pose.pose.position.x, pose.pose.position.y, pose.pose.position.z
]
rospy.loginfo_throttle(1.0, "Calculated pose: {}".format(pose))
self.tfb.sendTransform(position, orientation, pose.header.stamp,
child_frame, pose.header.frame_id)
odom = Odometry()
odom.header = pose.header
odom.pose = PoseWithCovariance(pose=pose.pose)
odom.pose.covariance[0] = -1
odom.twist = TwistWithCovariance(twist=twist.twist)
odom.twist.covariance[0] = -1
odom.child_frame_id = child_frame
self.odom_pub.publish(odom)
def straight_path():
path = Path()
goal_odom = Odometry()
#Vehicle linear speed in x, y, z in m/s
rospy.init_node('straight_path', anonymous=True)
t = rospy.get_time()
pathPub = rospy.Publisher('/path', Path, queue_size=1)
odom_pub = rospy.Publisher("/goal_odom", Odometry, queue_size=50)
rate = rospy.Rate(10) # 10hz
xp_target = rospy.get_param('target_destination', '150')
maximum_speed = rospy.get_param('maximum_speed', '2')
sin_gain = rospy.get_param('sin_gain', '20')
sin_freq = rospy.get_param('sin_freq', '0.05')
just_sim = rospy.get_param('just_sim', '0')
while not rospy.is_shutdown():
xpo = 0
ypo = 0
zpo = 0
wpo = 1
vx = 0
vy = 0
vz = 0
x2p = []
y2p = []
v2p = []
vx2p = []
vy2p = []
# vehicle angular velocity in rad/s
wx = 0
wy = 0
wz = 0
lenght = int(xp_target)
v_max = float(maximum_speed)
gain = float(sin_gain)
freq = float(sin_freq)
for xp in range(0, lenght):
yp = gain * math.sin(freq * xp)
zp = 0
x2p.append(xp)
y2p.append(yp)
if float(xp) == 0:
estimated_heading = 0
elif xp == 1:
estimated_heading = math.atan(float(yp) / float(xp))
else:
estimated_heading = math.atan(
float(y2p[xp] - y2p[xp - 1]) /
float(x2p[xp] - x2p[xp - 1]))
if xp == 0 or xp == lenght:
vx = 0
vy = 0
elif xp == 1 or xp == lenght - 1:
vx = 0.25 * v_max * math.cos(estimated_heading)
vy = 0.25 * v_max * math.sin(estimated_heading)
elif xp == 2 or xp == lenght - 2:
vx = 0.5 * v_max * math.cos(estimated_heading)
vy = 0.5 * v_max * math.sin(estimated_heading)
elif xp == 3 or xp == lenght - 3:
vx = 0.75 * v_max * math.cos(estimated_heading)
vy = 0.75 * v_max * math.sin(estimated_heading)
else:
vx = v_max * math.cos(estimated_heading)
vy = v_max * math.sin(estimated_heading)
vz = 0
vx2p.append(vx)
vy2p.append(vy)
if estimated_heading == 0:
v2p.append(vx)
else:
v2p.append(float(vy) / float(math.sin(estimated_heading)))
#v2p.append(vx)
#rospy.loginfo('***************** xp/xp_target: [%f]', float(xp)/float(xp_target))
quaternion = quaternion_from_euler(xpo, ypo, estimated_heading)
pose = PoseStamped()
poseWithCov = PoseWithCovariance()
twistWithCov = TwistWithCovariance()
pose.header.frame_id = "map"
pose.pose.position.x = float(xp)
pose.pose.position.y = float(yp)
pose.pose.position.z = float(zp)
pose.pose.orientation.x = float(quaternion[0])
pose.pose.orientation.y = float(quaternion[1])
pose.pose.orientation.z = float(quaternion[2])
pose.pose.orientation.w = float(quaternion[3])
poseWithCov.pose.position.x = float(xp)
poseWithCov.pose.position.y = float(yp)
poseWithCov.pose.position.z = float(zp)
poseWithCov.pose.orientation.x = float(quaternion[0])
poseWithCov.pose.orientation.y = float(quaternion[1])
poseWithCov.pose.orientation.z = float(quaternion[2])
poseWithCov.pose.orientation.w = float(quaternion[3])
twistWithCov.twist.linear.x = vx
twistWithCov.twist.linear.y = vy
twistWithCov.twist.linear.z = vz
twistWithCov.twist.angular.x = wx
twistWithCov.twist.angular.y = wy
twistWithCov.twist.angular.z = wz
pose.header.seq = path.header.seq + 1
path.header.frame_id = "map"
path.header.stamp = rospy.Time.now()
pose.header.stamp = path.header.stamp
goal_odom.header.frame_id = "map"
goal_odom.header.stamp = rospy.Time.now()
goal_odom.header.stamp = goal_odom.header.stamp
path.poses.append(pose)
goal_odom.pose = poseWithCov
goal_odom.twist = twistWithCov
odom_pub.publish(goal_odom)
#rospy.loginfo('***************** sin_freq: [%s]', sin_freq)
if float(just_sim) == 1:
pathPub.publish(path)
plt.plot(x2p, y2p, label='Y(m) vs X(m)')
plt.legend()
plt.show()
# WE may need to subscribe to localization ersult since we need heading toward X and Y axis to prepare vx and vy for twist data of the nav_msg/Odometry
# spin() simply keeps python from exiting until this node is stopped
#rospy.spin()
rate.sleep()