def _msg(self, quaternion, x_dist, y_dist, linear_speed, angular_speed, now, use_pose_ekf=False):
# next, we will publish the odometry message over ROS
msg = Odometry()
msg.header.frame_id = "odom"
msg.header.stamp = now
msg.pose.pose = Pose(Point(x_dist, y_dist, 0.0), quaternion)
msg.child_frame_id = "base_link"
msg.twist.twist = Twist(Vector3(linear_speed, 0, 0), Vector3(0, 0, angular_speed))
if use_pose_ekf:
# robot_pose_ekf needs these covariances and we may need to adjust them.
# From: ~/turtlebot/src/turtlebot_create/create_node/src/create_node/covariances.py
# However, this is not needed because we are not using robot_pose_ekf
# odometry.pose.covariance = [1e-3, 0, 0, 0, 0, 0,
# 0, 1e-3, 0, 0, 0, 0,
# 0, 0, 1e6, 0, 0, 0,
# 0, 0, 0, 1e6, 0, 0,
# 0, 0, 0, 0, 1e6, 0,
# 0, 0, 0, 0, 0, 1e3]
#
# odometry.twist.covariance = [1e-3, 0, 0, 0, 0, 0,
# 0, 1e-3, 0, 0, 0, 0,
# 0, 0, 1e6, 0, 0, 0,
# 0, 0, 0, 1e6, 0, 0,
# 0, 0, 0, 0, 1e6, 0,
# 0, 0, 0, 0, 0, 1e3]
pass
return msg
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 pubOdometry(self, event):
odom = Odometry()
odom.header.stamp = rospy.Time.now()
odom.header.frame_id = str(self.frame_id)
odom.child_frame_id = "real_girona500"
odom.pose.pose.position.x = self.p[0]
odom.pose.pose.position.y = self.p[1]
odom.pose.pose.position.z = self.p[2]
orientation = tf.transformations.quaternion_from_euler(self.p[3], self.p[4], self.p[5], 'sxyz')
odom.pose.pose.orientation.x = orientation[0]
odom.pose.pose.orientation.y = orientation[1]
odom.pose.pose.orientation.z = orientation[2]
odom.pose.pose.orientation.w = orientation[3]
#Haig de comentar les velocitats sino l'UWSim no va
odom.twist.twist.linear.x = 0.0 #v_[0]
odom.twist.twist.linear.y = 0.0 #v_[1]
odom.twist.twist.linear.z = 0.0 #v_[2]
odom.twist.twist.angular.x = 0.0 #v_[3]
odom.twist.twist.angular.y = 0.0 #v_[4]
odom.twist.twist.angular.z = 0.0 #v_[5]
self.pub_odom.publish(odom)
# Broadcast transform
br = tf.TransformBroadcaster()
br.sendTransform((self.p[0], self.p[1], self.p[2]), orientation,
odom.header.stamp, odom.header.frame_id, "world")
def publish_odom(event):
global motor_controller, od
pos = motor_controller.position
covariance = Config.get_covariance_matrix()
odom = Odometry()
odom.header.frame_id = "odom"
odom.child_frame_id = "base_link"
if rospy.get_time() - pos.position_time > 50.0/1000:
odom.header.stamp = rospy.Time.now()
else:
odom.header.stamp = rospy.Time.from_sec(pos.position_time)
odom.pose.pose.position.x = pos.x
odom.pose.pose.position.y = pos.y
odom.pose.pose.orientation.z = math.sin(pos.yaw / 2)
odom.pose.pose.orientation.w = math.cos(pos.yaw / 2)
odom.pose.covariance = covariance
odom.twist.twist.linear.x = pos.linear_vel
odom.twist.twist.angular.z = pos.angular_vel
odom.twist.covariance = covariance
odom_publisher.publish(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 tf_and_odom(self, zumo_msg):
"""Broadcast tf transform and publish odometry. It seems a little
redundant to do both, but both are needed if we want to use the ROS
navigation stack."""
trans = (zumo_msg.x, zumo_msg.y, 0)
rot = tf.transformations.quaternion_from_euler(0, 0, zumo_msg.theta)
self.broadcaster.sendTransform(trans, rot, \
rospy.Time.now(), \
"base_link", \
"odom")
odom = Odometry()
odom.header.frame_id = 'odom'
odom.header.stamp = rospy.Time.now()
odom.pose.pose.position.x = trans[0]
odom.pose.pose.position.y = trans[1]
odom.pose.pose.position.z = trans[2]
odom.pose.pose.orientation.x = rot[0]
odom.pose.pose.orientation.y = rot[1]
odom.pose.pose.orientation.z = rot[2]
odom.pose.pose.orientation.w = rot[3]
odom.child_frame_id = 'base_link'
odom.twist.twist = self.stored_twist
self.odomPublisher.publish(odom)
def update_vel():
"Main loop"""
global g_last_loop_time, g_vel_x, g_vel_y, g_vel_dt, x_pos, y_pos
#while not rospy.is_shutdown():
current_time = rospy.Time.now()
linear_velocity_x = g_vel_x
linear_velocity_y = g_vel_y
# Calculate current position of the robot
x_pos += linear_velocity_x * g_vel_dt
y_pos += linear_velocity_y * g_vel_dt
# All odometry is 6DOF, so need a quaternion created from yaw
odom_quat = tf.transformations.quaternion_from_euler(0, 0, 0)
# next, we'll publish the odometry message over ROS
odom = Odometry()
odom.header.stamp = current_time
odom.header.frame_id = "odom"
# set the position
odom.pose.pose = Pose(Point(x_pos, y_pos, 0.), Quaternion(*odom_quat))
# set the velocity
odom.child_frame_id = "base_footprint"
odom.twist.twist = Twist(
Vector3(linear_velocity_x, linear_velocity_y, 0), Vector3(0, 0, 0))
# publish the message
odom_pub.publish(odom)
g_last_loop_time = current_time
def handle_harlie_pose(msg, push_casters): pose = changePoseCoordFrame(msg) current_time = rospy.Time.now() theta = pose.theta + pi if push_casters else pose.theta quaternion = tf.transformations.quaternion_about_axis(theta, (0,0,1)) tf_br.sendTransform(translation = (pose.x, pose.y, 0), rotation = tuple(quaternion), time = current_time, child = 'base_link', parent = 'odom') odom_msg = Odometry() odom_msg.header.stamp = current_time odom_msg.header.frame_id = 'odom' odom_msg.pose.pose.position.x = pose.x odom_msg.pose.pose.position.y = pose.y odom_msg.pose.pose.position.z = 0.0 odom_msg.pose.pose.orientation = Quaternion(*quaternion) odom_msg.pose.covariance[0] = pose.x_var odom_msg.pose.covariance[7] = pose.y_var odom_msg.pose.covariance[35] = pose.theta_var odom_msg.child_frame_id = 'base_link' odom_msg.twist.twist.linear.x = pose.vel odom_msg.twist.twist.angular.z = pose.omega odom_msg.twist.covariance[0] = pose.vel_var odom_msg.twist.covariance[35] = pose.omega_var odom_pub.publish(odom_msg)
def Talk(self):
msgs = Odometry()
msgs.header.stamp = rospy.Time.now()
msgs.header.frame_id = '/nav'
msgs.child_frame_id = self.name
msgs.pose.pose.position.x = self.position.x
msgs.pose.pose.position.y = self.position.y
msgs.pose.pose.position.z = self.position.z
msgs.pose.pose.orientation.x = self.orientation.x
msgs.pose.pose.orientation.y = self.orientation.y
msgs.pose.pose.orientation.z = self.orientation.z
msgs.pose.pose.orientation.w = self.orientation.w
msgs.twist.twist.linear.x = self.velocity.x
msgs.twist.twist.linear.y = self.velocity.y
msgs.twist.twist.linear.z = self.velocity.z
#rospy.loginfo(msgs)
self.publisher['state'].publish(msgs)
self.tf_broadcaster['local_tf'].sendTransform( (msgs.pose.pose.position.x, msgs.pose.pose.position.y, msgs.pose.pose.position.z) ,
(msgs.pose.pose.orientation.x, msgs.pose.pose.orientation.y, msgs.pose.pose.orientation.z, msgs.pose.pose.orientation.w),
msgs.header.stamp,
msgs.child_frame_id,
msgs.header.frame_id)
def getOdometry(odometry_line):
# Timestamp [seconds.microseconds]
# Rolling Counter [signed 16bit integer]
# TicksRight [ticks]
# TicksLeft [ticks]
# X [m]*
# Y [m]*
# Theta [rad]*
# => 1235603339.32339, 4103, 2464, 2559, 1.063, 0.008, -0.028
str_x, str_y, str_th = odometry_line.split(', ')[4:7]
current_time = rospy.Time.now()
th = float(str_th)
x = float(str_x)
y = float(str_y)
z = 0.0
roll = 0
pitch = 0
yaw = th
qx, qy, qz, qw = tf.transformations.quaternion_from_euler(roll, pitch, yaw)
# BUILDING ODOMETRY
msg = Odometry()
msg.header.stamp = current_time
msg.header.frame_id = 'odom'
msg.child_frame_id = 'base_link'
msg.pose.pose.position = Point(x, y, z)
msg.pose.pose.orientation = Quaternion(qx, qy, qz, qw)
return msg
def handle_imu(self, imu_data):
if not self.imu_recv:
rospy.loginfo('IMU data received')
self.imu_recv = True
if self.pub.get_num_connections() != 0:
msg = Odometry()
#msg.header.frame_id = imu_data.header.frame_id
msg.header.frame_id = 'imu_frame'
msg.header.stamp = imu_data.header.stamp
msg.child_frame_id = 'imu_base_link'
msg.pose.pose.position.x = 0.0 # here, we can maybe try to integrate accelerations...
msg.pose.pose.position.y = 0.0
msg.pose.pose.position.z = 0.0
msg.pose.pose.orientation = imu_data.orientation # should we transform this orientation into base_footprint frame????
msg.pose.covariance = [99999, 0, 0, 0, 0, 0, # large covariance on x, y, z
0, 99999, 0, 0, 0, 0,
0, 0, 99999, 0, 0, 0,
0, 0, 0, 1e-06, 0, 0, # we trust in rpy
0, 0, 0, 0, 1e-06, 0,
0, 0, 0, 0, 0, 1e-06]
# does robot_pose_ekf care about twist?
self.pub.publish(msg)
def publish_Tfs():
global tf_rot,tf_trans,tf_tda,tf_rda,tf_tbc,tf_rbc,scale
br = tf.TransformBroadcaster()
rate = rospy.Rate(50.0)
while not rospy.is_shutdown():
rate.sleep()
camOdom = Odometry()
camOdom.header.stamp = rospy.Time.now()
camOdom.header.frame_id = "/odom_combined"
camOdom.pose.pose.position.x =0
camOdom.pose.pose.position.y =0
camOdom.pose.pose.position.z =0. #Tbc[2]
M=np.identity(4)
q=tf.transformations.quaternion_from_matrix(M)
camOdom.pose.pose.orientation.x = q[0]
camOdom.pose.pose.orientation.y = q[1]
camOdom.pose.pose.orientation.z = q[2]
camOdom.pose.pose.orientation.w = q[3]
camOdom.child_frame_id = "/base_footprint"
camOdom.twist.twist.linear.x = 0
camOdom.twist.twist.linear.y = 0
camOdom.twist.twist.angular.z = 0
T=np.array([0.,0.,0.])
M=np.identity(4)
q=tf.transformations.quaternion_from_matrix(M)
br.sendTransform(T,q,
rospy.Time.now(),
"/base_footprint",
"/odom_combined")
camOdomPub.publish(camOdom)
def poll(self):
(x, y, theta) = self.arduino.arbot_read_odometry()
quaternion = Quaternion()
quaternion.x = 0.0
quaternion.y = 0.0
quaternion.z = sin(theta / 2.0)
quaternion.w = cos(theta / 2.0)
# Create the odometry transform frame broadcaster.
now = rospy.Time.now()
self.odomBroadcaster.sendTransform(
(x, y, 0),
(quaternion.x, quaternion.y, quaternion.z, quaternion.w),
now,
"base_link",
"odom"
)
odom = Odometry()
odom.header.frame_id = "odom"
odom.child_frame_id = "base_link"
odom.header.stamp = now
odom.pose.pose.position.x = x
odom.pose.pose.position.y = y
odom.pose.pose.position.z = 0
odom.pose.pose.orientation = quaternion
odom.twist.twist.linear.x = self.forwardSpeed
odom.twist.twist.linear.y = 0
odom.twist.twist.angular.z = self.angularSpeed
self.arduino.arbot_set_velocity(self.forwardSpeed, self.angularSpeed)
self.odomPub.publish(odom)
def onNavSts(self,data):
q = tf.transformations.quaternion_from_euler(math.pi, 0, math.pi/2)
self.broadcaster.sendTransform((0,0,0), q.conjugate(), rospy.Time.now(), "uwsim_frame", "local");
try:
(trans,rot) = self.listener.lookupTransform("uwsim_frame", self.base_frame, rospy.Time(0))
odom = Odometry();
odom.twist.twist.linear.x = data.body_velocity.x;
odom.twist.twist.linear.y = data.body_velocity.y;
odom.twist.twist.linear.z = data.body_velocity.z;
odom.twist.twist.angular.x = data.orientation_rate.roll;
odom.twist.twist.angular.y = data.orientation_rate.pitch;
odom.twist.twist.angular.z = data.orientation_rate.yaw;
odom.child_frame_id = self.base_frame;
odom.pose.pose.orientation.x = rot[0];
odom.pose.pose.orientation.y = rot[1];
odom.pose.pose.orientation.z = rot[2];
odom.pose.pose.orientation.w = rot[3];
odom.pose.pose.position.x = trans[0];
odom.pose.pose.position.y = trans[1];
odom.pose.pose.position.z = trans[2];
odom.header.stamp = rospy.Time.now();
odom.header.frame_id = "local";
self.pub.publish(odom);
except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
None
def callback(msg):
pub = rospy.Publisher("odom_covar", Odometry)
corrected = Odometry()
corrected.header.seq = msg.header.seq
corrected.header.stamp = rospy.Time.now()
corrected.header.frame_id = msg.header.frame_id
corrected.child_frame_id = "base_footprint"
corrected.pose.pose = msg.pose.pose
corrected.pose.covariance = [.01, 0, 0, 0, 0, 0,
0, .01, 0, 0, 0, 0,
0, 0, .01, 0, 0, 0,
0, 0, 0, .01, 0, 0,
0, 0, 0, 0, .01, 0,
0, 0, 0, 0, 0, .01]
corrected.twist.twist = msg.twist.twist
corrected.twist.covariance = [.01, 0, 0, 0, 0, 0,
0, .01, 0, 0, 0, 0,
0, 0, .01, 0, 0, 0,
0, 0, 0, .01, 0, 0,
0, 0, 0, 0, .01, 0,
0, 0, 0, 0, 0, .01]
pub.publish(corrected)
def post_odometry(self, component_instance):
""" Publish the data of the Pose as a Odometry message for fake localization
"""
parent_name = component_instance.robot_parent.blender_obj.name
component_name = component_instance.blender_obj.name
frame_id = self._properties[component_name]['frame_id']
child_frame_id = self._properties[component_name]['child_frame_id']
odometry = Odometry()
odometry.header.stamp = rospy.Time.now()
odometry.header.frame_id = frame_id
odometry.child_frame_id = child_frame_id
odometry.pose.pose.position.x = component_instance.local_data['x']
odometry.pose.pose.position.y = component_instance.local_data['y']
odometry.pose.pose.position.z = component_instance.local_data['z']
euler = mathutils.Euler((component_instance.local_data['roll'],
component_instance.local_data['pitch'],
component_instance.local_data['yaw']))
quaternion = euler.to_quaternion()
odometry.pose.pose.orientation = quaternion
for topic in self._topics:
# publish the message on the correct topic
if str(topic.name) == str("/" + parent_name + "/" + component_name):
topic.publish(odometry)
def Talk(self, time):
#print self.orientation.get_euler()['yaw'], self.position.yaw, self.sensors['gps'].position.yaw
msgs = Odometry( )
msgs.header.stamp = rospy.Time.now()
msgs.header.frame_id = "/nav"
msgs.child_frame_id = "/drone_local"
msgs.pose.pose.position.x = self.position.x
msgs.pose.pose.position.y = self.position.y
msgs.pose.pose.position.z = self.position.z
msgs.pose.pose.orientation.x = self.orientation.x
msgs.pose.pose.orientation.y = self.orientation.y
msgs.pose.pose.orientation.z = self.orientation.z
msgs.pose.pose.orientation.w = self.orientation.w
msgs.twist.twist.linear.x = self.velocity.x
msgs.twist.twist.linear.y = self.velocity.y
msgs.twist.twist.linear.z = self.velocity.z
#rospy.loginfo(msgs)
self.publisher['state'].publish(msgs)
self.tf_broadcaster['drone_local_tf'].sendTransform( (msgs.pose.pose.position.x, msgs.pose.pose.position.y, msgs.pose.pose.position.z) ,
self.orientation.get_quaternion( ),
msgs.header.stamp,
msgs.child_frame_id,
msgs.header.frame_id)
def publish_odom(self, cur_x, cur_y, cur_theta, vx, vth):
quat = tf.transformations.quaternion_from_euler(0, 0, cur_theta)
current_time = rospy.Time.now()
br = tf.TransformBroadcaster()
br.sendTransform((cur_x, cur_y, 0),
tf.transformations.quaternion_from_euler(0, 0, cur_theta),
current_time,
"base_footprint",
"odom")
odom = Odometry()
odom.header.stamp = current_time
odom.header.frame_id = 'odom'
odom.pose.pose.position.x = cur_x
odom.pose.pose.position.y = cur_y
odom.pose.pose.position.z = 0.0
odom.pose.pose.orientation = Quaternion(*quat)
odom.pose.covariance[0] = 0.01
odom.pose.covariance[7] = 0.01
odom.pose.covariance[14] = 99999
odom.pose.covariance[21] = 99999
odom.pose.covariance[28] = 99999
odom.pose.covariance[35] = 0.01
odom.child_frame_id = 'base_footprint'
odom.twist.twist.linear.x = vx
odom.twist.twist.linear.y = 0
odom.twist.twist.angular.z = vth
odom.twist.covariance = odom.pose.covariance
self.odom_pub.publish(odom)
def publish_odom(self, robot):
# Transform etc
x = robot.x
y = robot.y
th = robot.th
br = tf.TransformBroadcaster()
quat = tf.transformations.quaternion_from_euler(0,0,th)
odom_trans = TransformStamped()
odom_trans.header.stamp = rospy.Time.now()
odom_trans.header.frame_id = "/map"
odom_trans.child_frame_id = "/base_link"
odom_trans.transform.translation.x = x
odom_trans.transform.translation.y = y
odom_trans.transform.translation.z = 0.0
odom_trans.transform.rotation = quat;
#tf.TransformBroadcaster().SendTransform(R,t,time,child,parent)
br.sendTransform((x,y,0.0), quat, odom_trans.header.stamp, odom_trans.child_frame_id, odom_trans.header.frame_id)
# Publish Odom msg
odom_msg = Odometry()
odom_msg.header.stamp = rospy.Time.now()
odom_msg.header.frame_id = "/map"
odom_msg.child_frame_id = "/base_link"
odom_msg.pose.pose.position.x = x
odom_msg.pose.pose.position.y = y
odom_msg.pose.pose.position.z = 0.0
odom_msg.pose.pose.orientation.x = quat[0]
odom_msg.pose.pose.orientation.y = quat[1]
odom_msg.pose.pose.orientation.z = quat[2]
odom_msg.pose.pose.orientation.w = quat[3]
# publish
self.odomPub.publish(odom_msg)
def main():
rospy.init_node("pose2d_to_odometry")
own_num = rospy.get_param('~own_num', 0)
global odom, head
head = Header()
head.stamp = rospy.Time.now()
head.frame_id = "robot_{}/odom_combined".format(own_num)
odom = Odometry()
odom.child_frame_id = "robot_{}/base_footprint".format(own_num)
o = 10**-9 # zero
odom.pose.covariance = [1, 0, 0, 0, 0, 0,
0, 1, 0, 0, 0, 0,
0, 0, o, 0, 0, 0, # z doesn't change
0, 0, 0, o, 0, 0, # constant roll
0, 0, 0, 0, o, 0, # constant pitch
0, 0, 0, 0, 0, o] # guess - .3 radians rotation cov
odom.twist.covariance = [100, 0, 0, 0, 0, 0,
0, 100, 0, 0, 0, 0,
0, 0, 100, 0, 0, 0, # large covariances - no data
0, 0, 0, 100, 0, 0,
0, 0, 0, 0, 100, 0,
0, 0, 0, 0, 0, 100]
global odom_pub
odom_topic = rospy.get_param('~odometry_publish_topic', 'vo')
pose2D_topic = rospy.get_param('~pose2D_topic', 'pose2D')
odom_pub = rospy.Publisher(odom_topic, Odometry)
rospy.Subscriber(pose2D_topic, Pose2D, on_pose)
rospy.spin()
def Publish_Odom_Tf(self): #quaternion = tf.transformations.quaternion_from_euler(0, 0, theta) quaternion = Quaternion() quaternion.x = 0 quaternion.y = 0 quaternion.z = math.sin(self.Heading / 2.0) quaternion.w = math.cos(self.Heading / 2.0) rosNow = rospy.Time.now() self._tf_broad_caster.sendTransform( (self.X_Pos, self.Y_Pos, 0), (quaternion.x, quaternion.y, quaternion.z, quaternion.w), rosNow, "base_footprint", "odom" ) # next, we'll publish the odometry message over ROS odometry = Odometry() odometry.header.frame_id = "odom" odometry.header.stamp = rosNow odometry.pose.pose.position.x = self.X_Pos odometry.pose.pose.position.y = self.Y_Pos odometry.pose.pose.position.z = 0 odometry.pose.pose.orientation = quaternion odometry.child_frame_id = "base_link" odometry.twist.twist.linear.x = self.Velocity odometry.twist.twist.linear.y = 0 odometry.twist.twist.angular.z = self.Omega self._odom_publisher.publish(odometry)
def talker(message):
corrected = Odometry()
corrected.header.seq = message.header.seq
corrected.header.stamp = rospy.Time.now()
corrected.header.frame_id = message.header.frame_id #base_footprint #message.header.frame_id
corrected.child_frame_id = message.child_frame_id #"base_footprint"
corrected.pose.pose = message.pose.pose
corrected.pose.covariance = [0.05, 0, 0, 0, 0, 0,
0, 0.05, 0, 0, 0, 0,
0, 0, 0.05, 0, 0, 0,
0, 0, 0, 0.001, 0, 0,
0, 0, 0, 0, 0.001, 0,
0, 0, 0, 0, 0, .5]
corrected.twist.twist = message.twist.twist
corrected.twist.covariance = [0.05, 0, 0, 0, 0, 0,
0, 0.05, 0, 0, 0, 0,
0, 0, 0.05, 0, 0, 0,
0, 0, 0, 0.001, 0, 0,
0, 0, 0, 0, 0.001, 0,
0, 0, 0, 0, 0, .5]
pub = rospy.Publisher('odom_w_cov', Odometry)
pub.publish(corrected)
def listener():
rospy.init_node('listener', anonymous=True)
rospy.Subscriber("/wiimote1/imu", Imu, wiimote1Callback)
rospy.Subscriber("/wiimote2/imu", Imu, wiimote2Callback)
odomPub = rospy.Publisher("/base_controller/odom", Odometry)
odom = Odometry()
while 1:
br = tf.TransformBroadcaster()
time.sleep(.01)
robot.updateState()
if robot.readyToPublish:
quaternion = tf.transformations.quaternion_from_euler(0, 0, robot.theta)
#tf msg
#br.sendTransform((0, 0, 0), tf.transformations.quaternion_from_euler(0, 0, 0),rospy.Time.now(),"/odom", "/map")
br.sendTransform((robot.x, robot.y, 0), tf.transformations.quaternion_from_euler(0, 0, robot.theta),rospy.Time.now(),"/base_link", "/odom")
br.sendTransform((0, 0, .7), tf.transformations.quaternion_from_euler(0, 0, 0),rospy.Time.now(),"/base_laser_link", "/base_link")
br.sendTransform((0, 0, 0), tf.transformations.quaternion_from_euler(0, 0, 0),rospy.Time.now(),"/camera1_link", "/base_laser_link")
br.sendTransform((0, 0, 0), tf.transformations.quaternion_from_euler(0, 0, 3.14),rospy.Time.now(),"/camera2_link", "/base_laser_link")
robot.readyToPublish = False
#odometry msg
odom.header.stamp = robot.lastTime
odom.child_frame_id = "base_link"
odom.pose.pose.position.x = robot.x
odom.pose.pose.position.y = robot.y
odom.pose.pose.position.z = 0
odom.pose.pose.orientation.x = quaternion[0]
odom.pose.pose.orientation.y = quaternion[1]
odom.pose.pose.orientation.z = quaternion[2]
odom.pose.pose.orientation.w = quaternion[3]
odom.twist.twist.linear.x = robot.xV
odom.twist.twist.linear.y = robot.yV
# the x angular velocity corresponds to left wheel and y to right wheel
odom.twist.twist.angular.x = robot.leftV
odom.twist.twist.angular.y = robot.rightV
odom.twist.twist.angular.z = robot.thetaV
# place holder covariance matrix
P = [.0001, 0, 0, 0, 0, 0,
0, .0001, 0, 0, 0, 0,
0, 0, .0001, 0, 0, 0,
0, 0, 0, .0001, 0, 0,
0, 0, 0, 0, .0001, 0,
0, 0, 0, 0, 0, .0001]
odom.pose.covariance = P
odomPub.publish(odom)
# spin() simply keeps python from exiting until this node is stopped
rospy.spin()
def publish(self, data):
q = Quaternion()
q.x = 0
q.y = 0
q.z = data[6]
q.w = data[7]
odomMsg = Odometry()
odomMsg.header.frame_id = self._odom
odomMsg.header.stamp = rospy.get_rostime()
odomMsg.child_frame_id = self._baseLink
odomMsg.pose.pose.position.x = data[0]
odomMsg.pose.pose.position.y = data[1]
odomMsg.pose.pose.position.z = 0
odomMsg.pose.pose.orientation = q
if self._firstTimePub:
self._prevOdom = odomMsg
self._firstTimePub = False
return
velocity = Twist()
deltaTime = odomMsg.header.stamp.to_sec() - self._prevOdom.header.stamp.to_sec()
yaw, pitch, roll = euler_from_quaternion(
[odomMsg.pose.pose.orientation.w, odomMsg.pose.pose.orientation.x, odomMsg.pose.pose.orientation.y,
odomMsg.pose.pose.orientation.z])
prevYaw, prevPitch, prevRollprevYaw = euler_from_quaternion(
[self._prevOdom.pose.pose.orientation.w, self._prevOdom.pose.pose.orientation.x,
self._prevOdom.pose.pose.orientation.y, self._prevOdom.pose.pose.orientation.z])
if deltaTime > 0:
velocity.linear.x = (data[8] / deltaTime)
deltaYaw = yaw - prevYaw
# rospy.loginfo("yaw: %f\t\tpevYaw: %f\t\tdeltaYaw: %f" % (yaw,prevYaw,deltaYaw))
if deltaYaw > math.pi: deltaYaw -= 2 * math.pi
elif deltaYaw < -math.pi: deltaYaw += 2 * math.pi
if deltaTime > 0:
velocity.angular.z = -(deltaYaw / deltaTime)
# rospy.loginfo("deltaYaw after check: %f\t\t angular: %f" % (deltaYaw, velocity.angular.z))
odomMsg.twist.twist = velocity
self._prevOdom = odomMsg
traMsg = TransformStamped()
traMsg.header.frame_id = self._odom
traMsg.header.stamp = rospy.get_rostime()
traMsg.child_frame_id = self._baseLink
traMsg.transform.translation.x = data[0]
traMsg.transform.translation.y = data[1]
traMsg.transform.translation.z = 0
traMsg.transform.rotation = q
self._pub.publish(odomMsg)
self._broadCase.sendTransformMessage(traMsg)
def inspvax_handler(self, inspvax):
# Convert the latlong to x,y coordinates and publish an Odometry
try:
utm_pos = geodesy.utm.fromLatLong(inspvax.latitude, inspvax.longitude)
except ValueError:
# Probably coordinates out of range for UTM conversion.
return
if not self.init and self.zero_start:
self.origin.x = utm_pos.easting
self.origin.y = utm_pos.northing
self.origin.z = inspvax.altitude
self.pub_origin.publish(position=self.origin)
odom = Odometry()
odom.header.stamp = rospy.Time.now()
odom.header.frame_id = self.odom_frame
odom.child_frame_id = self.base_frame
odom.pose.pose.position.x = utm_pos.easting - self.origin.x
odom.pose.pose.position.y = utm_pos.northing - self.origin.y
odom.pose.pose.position.z = inspvax.altitude - self.origin.z
# Orientation
# Save this on an instance variable, so that it can be published
# with the IMU message as well.
self.orientation = tf.transformations.quaternion_from_euler(
radians(inspvax.pitch), radians(inspvax.roll), radians(90 - inspvax.azimuth)
)
print inspvax.azimuth
odom.pose.pose.orientation = Quaternion(*self.orientation)
odom.pose.covariance[21] = self.orientation_covariance[0] = pow(2, inspvax.pitch_std)
odom.pose.covariance[28] = self.orientation_covariance[4] = pow(2, inspvax.roll_std)
odom.pose.covariance[35] = self.orientation_covariance[8] = pow(2, inspvax.azimuth_std)
# Twist is relative to vehicle frame
odom.twist.twist.linear.x = inspvax.east_velocity
odom.twist.twist.linear.y = inspvax.north_velocity
odom.twist.twist.linear.z = inspvax.up_velocity
TWIST_COVAR[0] = pow(2, inspvax.east_velocity_std)
TWIST_COVAR[7] = pow(2, inspvax.north_velocity_std)
TWIST_COVAR[14] = pow(2, inspvax.up_velocity_std)
# odom.twist.twist.angular = imu.angular_velocity
odom.twist.covariance = TWIST_COVAR
self.pub_odom.publish(odom)
# Odometry transform (if required)
if self.publish_tf:
self.tf_broadcast.sendTransform(
(odom.pose.pose.position.x, odom.pose.pose.position.y, odom.pose.pose.position.z),
self.orientation,
odom.header.stamp,
odom.child_frame_id,
odom.header.frame_id,
)
# Mark that we've received our first fix, and set origin if necessary.
self.init = True
def run(self):
rosRate = rospy.Rate(self.rate)
rospy.loginfo("Publishing Odometry data at: " + str(self.rate) + " Hz")
vx = 0.0
vy = 0.0
vth = 0.0
while not rospy.is_shutdown() and not self.finished.isSet():
now = rospy.Time.now()
if now > self.t_next:
dt = now - self.then
self.then = now
dt = dt.to_sec()
delta_x = (vx * cos(self.th) - vy * sin(self.th)) * dt
delta_y = (vx * sin(self.th) + vy * cos(self.th)) * dt
delta_th = vth * dt
self.x += delta_x
self.y += delta_y
self.th += delta_th
quaternion = Quaternion()
quaternion.x = 0.0
quaternion.y = 0.0
quaternion.z = sin(self.th / 2.0)
quaternion.w = cos(self.th / 2.0)
# Create the odometry transform frame broadcaster.
self.odomBroadcaster.sendTransform(
(self.x, self.y, 0),
(quaternion.x, quaternion.y, quaternion.z, quaternion.w),
rospy.Time.now(),
"base_link",
"odom"
)
odom = Odometry()
odom.header.frame_id = "odom"
odom.child_frame_id = "base_link"
odom.header.stamp = now
odom.pose.pose.position.x = self.x
odom.pose.pose.position.y = self.y
odom.pose.pose.position.z = 0
odom.pose.pose.orientation = quaternion
odom.twist.twist.linear.x = vx
odom.twist.twist.linear.y = 0
odom.twist.twist.angular.z = vth
self.odomPub.publish(odom)
self.t_next = now + self.t_delta
rosRate.sleep()
def advance(self, dt):
now = self.then + dt
elapsed = now - self.then
self.then = now
if self.debug:
print "advancing to t=%5.3f" % (self.then)
ldist = dt * self.lwSpeed
rdist = dt * self.rwSpeed
d = (ldist + rdist)/2.0
th = (rdist - ldist)/self.distanceBetweenWheels
dx = d / dt
dth = th / dt
if (d != 0):
x = math.cos(th)*d
y = math.sin(th)*d # Does this differ from the create driver? Should it?
self.x = self.x + (math.cos(self.th)*x - math.sin(self.th)*y)
self.y = self.y + (math.sin(self.th)*x + math.cos(self.th)*y)
if (th != 0):
self.th = self.th + th
self.th = self.th % (2 * math.pi)
self.th = self.th - (2 * math.pi) if self.th > math.pi else self.th
quaternion = Quaternion()
quaternion.x = 0.0
quaternion.y = 0.0
quaternion.z = math.sin(self.th/2.0)
quaternion.w = math.cos(self.th/2.0)
# Most of the applications I'm working on don't need this.
# self.odomBroadcaster.sendTransform(
# (self.x, self.y, 0),
# (quaternion.x, quaternion.y, quaternion.z, quaternion.w),
# rospy.Time.now(),
# "base_link",
# "odom"
# )
odom = Odometry()
odom.header.stamp = rospy.Time.now()
odom.header.frame_id = "odom"
odom.pose.pose.position.x = self.x
odom.pose.pose.position.y = self.y
odom.pose.pose.position.z = 0
odom.pose.pose.orientation = quaternion
odom.child_frame_id = "base_link"
odom.twist.twist.linear.x = dx
odom.twist.twist.linear.y = 0
odom.twist.twist.angular.z = dth
self.odomPub.publish(odom)
def publish_odometry(self):
quat = tf.transformations.quaternion_from_euler(0, 0, self.yaw)
quaternion = Quaternion()
quaternion.x = quat[0]
quaternion.y = quat[1]
quaternion.z = quat[2]
quaternion.w = quat[3]
if self.last_time is None:
self.last_time = rospy.Time.now()
vx = self.vx #(msg->twist.twist.linear.x) *trans_mul_;
vy = self.vy #msg->twist.twist.linear.y;
vth = self.vyaw #(msg->twist.twist.angular.z) *rot_mul_;
current_time = rospy.Time.now()
dt = (current_time - self.last_time).to_sec()
delta_x = (vx * math.cos(self.yaw) - vy * math.sin(self.yaw)) * dt
delta_y = (vx * math.sin(self.yaw) + vy * math.cos(self.yaw)) * dt
delta_th = vth * dt
self.x += delta_x
self.y += delta_y
self.yaw += delta_th
odom = Odometry()
odom.header.stamp = rospy.Time.now()
odom.header.frame_id = "odom"
odom.child_frame_id = "base_link"
odom.pose.pose.position.x = self.x
odom.pose.pose.position.y = self.y
odom.pose.pose.position.z = 0.0
odom.pose.pose.orientation = quaternion
odom.twist.twist.linear.x = vx
odom.twist.twist.linear.y = vy
odom.twist.twist.angular.z = vth
self.odom_pub.publish(odom)
transform_stamped = TransformStamped()
transform_stamped.header = odom.header
transform_stamped.transform.translation.x = self.x
transform_stamped.transform.translation.y = self.y
transform_stamped.transform.translation.z = 0.0
transform_stamped.transform.rotation = quaternion
#self.tfbr.sendTransform(transform_stamped)
self.tfbr.sendTransform((self.x, self.y, 0.0),
(quat[0], quat[1], quat[2], quat[3]),
rospy.Time.now(),
"odom",
"base_link")
self.last_time = current_time
def update(self):
#############################################################################
now = rospy.Time.now()
if now > self.t_next:
elapsed = now - self.then
self.then = now
elapsed = elapsed.to_sec()
# this approximation works (in radians) for small angles
th = self.th - self.th_pre
self.dr = th / elapsed
# publish the odom information
quaternion = Quaternion()
quaternion.x = self.qx
quaternion.y = self.qy
quaternion.z = self.qz
quaternion.w = self.qw
self.odomBroadcaster.sendTransform(
(self.x, self.y, 0),
(0, 0, quaternion.z, quaternion.w),
rospy.Time.now(),
self.base_frame_id,
self.odom_frame_id,
)
self.laserBroadcaster.sendTransform(
(0, 0, 0), (0, 0, 0, 1), rospy.Time.now(), self.laser_frame_id, self.base_frame_id
)
odom = Odometry()
odom.header.stamp = now
odom.header.frame_id = self.odom_frame_id
odom.pose.pose.position.x = self.x
odom.pose.pose.position.y = self.y
odom.pose.pose.position.z = 0
odom.pose.pose.orientation = quaternion
odom.child_frame_id = self.base_frame_id
odom.twist.twist.linear.x = self.dx
odom.twist.twist.linear.y = 0
odom.twist.twist.angular.z = self.dr
self.odomPub.publish(odom)
laser = LaserScan()
laser.header.stamp = now
laser.header.frame_id = self.laser_frame_id
laser.angle_min = self.laser.angle_min
laser.angle_max = self.laser.angle_max
laser.angle_increment = self.laser.angle_increment
laser.time_increment = self.laser.time_increment
laser.scan_time = self.laser.scan_time
laser.range_min = self.laser.range_min
laser.range_max = self.laser.range_max
laser.ranges = self.laser.ranges
laser.intensities = self.laser.intensities
self.laserPub.publish(laser)
def _BroadcastOdometryInfo(self, lineParts):
partsCount = len(lineParts)
#rospy.logwarn(partsCount)
if (partsCount < 6):
pass
try:
x = int(lineParts[1]) / 1000.0
y = int(lineParts[2]) / 1000.0
theta = int(lineParts[3]) / 1000.0
vx = int(lineParts[4]) / 1000.0
omega = int(lineParts[5]) / 1000.0
#quaternion = tf.transformations.quaternion_from_euler(0, 0, theta)
quaternion = Quaternion()
quaternion.x = 0.0
quaternion.y = 0.0
quaternion.z = sin(theta / 2.0)
quaternion.w = cos(theta / 2.0)
rosNow = rospy.Time.now()
# First, we'll publish the transform from frame odom to frame base_link over tf
# Note that sendTransform requires that 'to' is passed in before 'from' while
# the TransformListener' lookupTransform function expects 'from' first followed by 'to'.
self._OdometryTransformBroadcaster.sendTransform(
(x, y, 0),
(quaternion.x, quaternion.y, quaternion.z, quaternion.w),
rosNow,
"base_link",
"odom"
)
# next, we'll publish the odometry message over ROS
odometry = Odometry()
odometry.header.frame_id = "odom"
odometry.header.stamp = rosNow
odometry.pose.pose.position.x = x
odometry.pose.pose.position.y = y
odometry.pose.pose.position.z = 0
odometry.pose.pose.orientation = quaternion
odometry.child_frame_id = "base_link"
odometry.twist.twist.linear.x = vx
odometry.twist.twist.linear.y = 0
odometry.twist.twist.angular.z = omega
self._OdometryPublisher.publish(odometry)
#rospy.loginfo(odometry)
except:
rospy.logwarn("Unexpected error:" + str(sys.exc_info()[0]))
def read_data(self, event):
data_str = self.ard.readline()
data_list = data_str.split()
try:
data_list = [float(i) / 100 for i in data_list]
except ValueError:
return # incorrect data
#print data_list
if len(data_list) != 4:
return # incorrect array size
if not in_range(data_list, -5.0, 5.0): # 36RPM -> 3.7699 rad/s
return # data not in range
self.w_fl, self.w_fr, self.w_rl, self.w_rr = data_list
# dead reckoning
dt = rospy.Time.now().to_sec() - self.time.to_sec() # time difference
self.time = rospy.Time.now() # update time
self.v_x = (self.w_fl + self.w_fr + self.w_rl + self.w_rr) * R / 4
self.v_y = (-self.w_fl + self.w_fr + self.w_rl - self.w_rr) * R / 4
self.omega = (-self.w_fl + self.w_fr - self.w_rl +
self.w_rr) * R / 4 / (LX + LY)
print[self.v_x, self.v_y, self.omega]
self.x = self.x + self.v_x * dt
self.y = self.y + self.v_y * dt
self.heading = self.heading + self.omega * dt
if self.pub_tf:
self.tf_br.sendTransform(
(self.x, self.y, 0),
(0, 0, sin(self.heading / 2), cos(self.heading / 2)),
rospy.Time.now(), 'car_base', 'odom')
# Publish odometry message
odom = Odometry()
odom.header.frame_id = 'odom'
odom.child_frame_id = 'car_base'
odom.pose.pose.orientation.z = sin(self.heading / 2)
odom.pose.pose.orientation.w = cos(self.heading / 2)
odom.twist.twist.linear.x = self.v_x
odom.twist.twist.linear.y = self.v_y
self.pub_odom.publish(odom)
def publish_odom(x, pub):
"""
Convert quad state into an odometry message and publish it.
:param x:
:param pub:
"""
# Roll pitch yaw trust
odom = Odometry()
odom.pose.pose.position.x = x[0]
odom.pose.pose.position.y = x[1]
odom.pose.pose.position.z = x[2]
# Velocities
odom.twist.twist.linear.x = x[3]
odom.twist.twist.linear.y = x[4]
odom.twist.twist.linear.z = x[5]
# Orientation
odom.pose.pose.orientation.x = x[6]
odom.pose.pose.orientation.y = x[7]
odom.pose.pose.orientation.z = x[8]
odom.pose.pose.orientation.w = x[9]
odom.pose.pose.orientation.x = x[7]
odom.pose.pose.orientation.y = x[8]
odom.pose.pose.orientation.z = x[9]
odom.pose.pose.orientation.w = x[6]
# Angular velocities
odom.twist.twist.angular.x = x[10]
odom.twist.twist.angular.y = x[11]
odom.twist.twist.angular.z = x[12]
odom.child_frame_id = 'modquad'
odom.header.frame_id = 'world'
pub.publish(odom)
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 callback_odom(self, msg):
x = msg.pose.pose.position.x
y = msg.pose.pose.position.y
z = msg.pose.pose.position.z
(r, p, y) = tf.transformations.euler_from_quaternion([
msg.pose.pose.orientation.x, msg.pose.pose.orientation.y,
msg.pose.pose.orientation.z, msg.pose.pose.orientation.w
])
# (r, p, y) = (msg.pose.pose.orientation.x, msg.pose.pose.orientation.y, msg.pose.pose.orientation.z)
meas = np.array([x, y, z, r, p, y])
self.buffer_meas[self.pointer, :] = meas
self.pointer = self.pointer + 1
if self.pointer == self.n_meas:
self.full_buffer = True
self.pointer = 0
if self.full_buffer:
A = np.cov(self.buffer_meas.T)
for a in A:
print(a)
print("------------")
cov_mat = np.cov(self.buffer_meas.T).flatten()
output = Odometry()
output.header.frame_id = 'map'
output.child_frame_id = 'zed_center'
output.header.stamp = rospy.get_rostime()
output.pose.pose = copy.deepcopy(msg.pose.pose)
output.pose.covariance = cov_mat.tolist()
pub_vo = rospy.Publisher('/vo_fixed', Odometry, queue_size=10)
pub_vo.publish(output)
def pub_odom(self):
self.tf_config()
self.current_time = rospy.Time.now()
dt = (self.current_time - self.last_time).to_sec()
'''if(self.z!=0):
delta_th = self.vth * dt
self.th = self.th + delta_th'''
delta_x = (self.vx * cos(self.th)) * dt # - self.vy * sin(self.th)
delta_y = (self.vx * sin(self.th)) * dt # + self.vy * cos(self.th)
delta_th = self.vth * dt
self.th = self.th + delta_th
yaw = self.th * (180 / pi)
self.x = self.x + delta_x
self.y = self.y + delta_y
self.last_time = self.current_time
odom_quat = tf.transformations.quaternion_from_euler(0, 0, self.th)
# publish odom
odom = Odometry()
odom.header.stamp = self.current_time
odom.header.frame_id = "odom"
odom.pose.pose = Pose(Point(self.x, self.y, 0), Quaternion(*odom_quat))
odom.child_frame_id = "base_link"
odom.twist.twist = Twist(Vector3(self.vx, 0, 0),
Vector3(0, 0, self.vth))
self.odom_pub.publish(odom)
# publish tf
# rospy.loginfo("pub_tf: " + str(self.x) + ", "+ str(self.y) +", " +str(yaw))
'''self.tf_broadcaster.sendTransform(
def pub_est(self,loc_x,loc_y,loc_z):
# pack up estimate to ROS msg and publish
"""
location=Vector3Stamped()
location.header.stamp = rospy.Time.now()
location.vector.x = self.xhat[0]
location.vector.y = self.xhat[1]
location.vector.z = self.xhat[2]
self.pub_location.publish(location)
"""
xhat_odom = Odometry()
xhat_odom.header.stamp = rospy.Time.now()
xhat_odom.header.frame_id="world"
xhat_odom.child_frame_id="world"
xhat_odom.pose.pose.position.x = loc_x # pn
xhat_odom.pose.pose.position.y = loc_y # pe
xhat_odom.pose.pose.position.z = loc_z # pd
#quat = tf.transformations.quaternion_from_euler(self.xhat[6].copy(), self.xhat[7].copy(), self.xhat[8].copy())
xhat_odom.pose.pose.orientation.x = 0
xhat_odom.pose.pose.orientation.y = 0
xhat_odom.pose.pose.orientation.z = 0.707107
xhat_odom.pose.pose.orientation.w = 0.707107
#xhat_odom.twist.twist.linear.x = 0 # u
#xhat_odom.twist.twist.linear.y = 0 # v
#xhat_odom.twist.twist.linear.z = 0 # w
estimate_pub_.publish(xhat_odom)
pose_temp=PoseStamped()
pose_temp.header=xhat_odom.header
pose_temp.pose=xhat_odom.pose.pose
groundtruth_path.header = pose_temp.header
groundtruth_path.poses.append(pose_temp)
pub1_.publish(groundtruth_path)
def main():
#r = rospy.Rate(10)
while not rospy.is_shutdown():
str = ard.readline()
print str
list = str.split(' ')
if len(list) == 6:
try:
x = float(list[0])
y = float(list[1])
theta = float(list[2])
v_x = float(list[3])
v_y = float(list[4])
omega = float(list[5])
except ValueError:
pass
#print x, y, theta, v_x, v_y, omega
quaternion = tf.transformations.quaternion_from_euler(0, 0, theta)
odom_br.sendTransform((x, y, 0), quaternion, rospy.Time.now(),
"odom", "base_link")
odom = Odometry()
odom.header.stamp = rospy.Time.now()
odom.header.frame_id = "odom"
odom.pose.pose.position.x = x
odom.pose.pose.position.y = y
odom.pose.pose.position.z = 0
odom.pose.pose.orientation.x = quaternion[0]
odom.pose.pose.orientation.y = quaternion[1]
odom.pose.pose.orientation.z = quaternion[2]
odom.pose.pose.orientation.w = quaternion[3]
odom.child_frame_id = "base_link"
odom.twist.twist.linear.x = v_x
odom.twist.twist.linear.y = v_y
#odom.twist.twist.linear.z = 0
#odom.twist.twist.angular.x = 0
#odom.twist.twist.angular.y
odom.twist.twist.angular.z = omega
odom_pub.publish(odom)
def cmdvel_to_Odm(v_x,v_th,dt,now_time):
global x
global y
global theta
#-------------------------
x += v_x * math.cos(theta) * dt
y += v_x * math.sin(theta) * dt
theta += v_th * dt
#-------------------------
odm_Quaternion = tf.transformations.quaternion_from_euler(0,0,theta)
##print(odm_Quaternion)
odm = Odometry()
##set the header
odm.header.stamp = now_time
odm.header.frame_id = "odom"
##set the position
odm.pose.pose.position.x = x
odm.pose.pose.position.y = y
odm.pose.pose.position.z = 0.0
## set the orientation
odm.pose.pose.orientation.x = odm_Quaternion[0]
odm.pose.pose.orientation.y = odm_Quaternion[1]
odm.pose.pose.orientation.z = odm_Quaternion[2]
odm.pose.pose.orientation.w = odm_Quaternion[3]
##set the velocity
odm.child_frame_id = "base_link"
odm.twist.twist.linear.x = v_x
odm.twist.twist.angular.z = v_th
pubOdm.publish(odm)
print("+++++++++ Odmetry ++++++++++")
print("x : %f" % x)
print("theta : %f" % theta)
return 0
def odometry_raw_cb(args):
odom = Odometry()
x, y, theta = tuple(args)
current_time = rospy.get_rostime()
odom.header.stamp.secs = current_time.secs
odom.header.stamp.nsecs = current_time.nsecs
odom.header.frame_id = "map"
odom.child_frame_id = "base_link"
odom.pose.pose.position.x = x
odom.pose.pose.position.y = y
odom.pose.pose.position.z = 0
quat = tf.transformations.quaternion_from_euler(0, 0, theta)
odom.pose.pose.orientation.x = quat[0]
odom.pose.pose.orientation.y = quat[1]
odom.pose.pose.orientation.z = quat[2]
odom.pose.pose.orientation.w = quat[3]
global pub
pub.publish(odom)
def handle_robot_pose(poseMsg, currTime):
br = tf.TransformBroadcaster() # create a broadcaster
theta = tf.transformations.quaternion_from_euler(0, 0, poseMsg[2])
odom = Odometry()
odom.header.stamp = currTime
odom.header.frame_id = "odom"
#set the position
odom.pose.pose.position = Point(poseMsg[0], poseMsg[1], 0)
odom.pose.pose.orientation = theta
#set the velocity
odom.child_frame_id = "base_link"
#odom.twist.twist.linear.x = 0
#odom.twist.twist.linear.y = 0
#odom.twist.twist.angular.z = 0
odomPub = rospy.Publisher('odom', Odometry, queue_size=1)
odomPub.publish(odom)
br.sendTransform((poseMsg[0], poseMsg[1], 0.0), theta, currTime,
"base_link", "odom") # publish it on ros
#create odometry msg
print "transform sent"
def alvarcb(self, markers):
rospy.logdebug("Detected markers!")
# can we find the correct marker?
for m in markers.markers:
if m.id == self.marker_id:
odom_meas = Odometry()
odom_meas.header.frame_id = self.frame_id
m.pose.header.frame_id = self.camera_frame_id
odom_meas.child_frame_id = self.base_frame_id
odom_meas.header.stamp = m.header.stamp
m.pose.header.stamp = m.header.stamp
# now we need to transform this pose measurement from the camera
# frame into the frame that we are reporting measure odometry in
pose_transformed = self.transform_pose(m.pose)
if pose_transformed is not None:
odom_meas.pose.pose = pose_transformed.pose
# Now let's add our offsets:
odom_meas = odom_conversions.odom_add_offset(
odom_meas, self.odom_offset)
self.meas_pub.publish(odom_meas)
self.send_transforms(odom_meas)
self.publish_path(m.pose)
return
def write_odometry(ts_ns, T_XB30_XB3k, bag):
ros_timestamp = nanoseconds_to_ros_timestamp(ts_ns)
T_B0_Bk = get_T_B_XB3() * T_XB30_XB3k * get_T_B_XB3().inverse()
rospose = Odometry()
rospose.child_frame_id = "B"
rospose.header.stamp = ros_timestamp
rospose.pose.pose.position.x = T_B0_Bk.getPosition()[0]
rospose.pose.pose.position.y = T_B0_Bk.getPosition()[1]
rospose.pose.pose.position.z = T_B0_Bk.getPosition()[2]
rospose.pose.pose.orientation.x = T_B0_Bk.getRotation().x()
rospose.pose.pose.orientation.y = T_B0_Bk.getRotation().y()
rospose.pose.pose.orientation.z = T_B0_Bk.getRotation().z()
rospose.pose.pose.orientation.w = T_B0_Bk.getRotation().w()
rospose.twist.twist.linear.x = 0.0
rospose.twist.twist.linear.y = 0.0
rospose.twist.twist.linear.z = 0.0
rospose.twist.twist.angular.x = 0.0
rospose.twist.twist.angular.y = 0.0
rospose.twist.twist.angular.z = 0.0
bag.write('/visual_odometry', rospose, t=ros_timestamp)
def _link_states_cb(self, msg):
"""
:param msg:
:type msg: LinkStates
:return:
"""
if self._sphere_link_name is None:
for i, name in enumerate(msg.name):
if "sphere" in name.lower():
self._sphere_link_name = name
self._sphere_link_index = i
break
else:
odom = Odometry()
odom.header.stamp = rospy.Time.now()
odom.header.frame_id = "odom"
odom.child_frame_id = "sphere"
odom.pose.pose = msg.pose[self._sphere_link_index]
odom.pose.covariance = [0]*36
odom.twist.twist = msg.twist[self._sphere_link_index]
odom.twist.covariance = [0]*36
self.odometry_publisher.publish(odom)
def save_odom_bag(seq, seconds, nanoseconds, v_x, v_y, v_ang, x, y, orientation): odom_msg = Odometry() odom_msg.header.seq = seq odom_msg.header.stamp.secs = seconds odom_msg.header.stamp.nsecs = nanoseconds odom_msg.header.frame_id = "odom" odom_topic = "/odom" odom_msg.child_frame_id = "base_link" odom_msg.pose.pose.position.x = x odom_msg.pose.pose.position.y = y odom_msg.pose.pose.position.z = 0 odom_msg.pose.pose.orientation.x = orientation[0] odom_msg.pose.pose.orientation.y = orientation[1] odom_msg.pose.pose.orientation.z = orientation[2] odom_msg.pose.pose.orientation.w = orientation[3] odom_msg.twist.twist.linear.x = v_x odom_msg.twist.twist.linear.y = v_y odom_msg.twist.twist.linear.z = 0 odom_msg.twist.twist.angular.x = 0 odom_msg.twist.twist.angular.y = 0 odom_msg.twist.twist.angular.z = v_ang bag.write(odom_topic, odom_msg, odom_msg.header.stamp)
def publish_se2(self, event):
if self.current_x and self.current_y and self.current_yaw and self.okay_to_publish:
se2 = SE2()
se2.x = self.current_x
se2.y = self.current_y
se2.yaw = self.current_yaw
self.se2_pub.publish(se2)
odom = Odometry()
header = Header()
header.stamp = rospy.Time.now()
header.frame_id = "odom"
odom.header = header
odom.child_frame_id = "base_link"
pose = PoseWithCovariance()
pose.pose.position.x = self.current_x
pose.pose.position.y = self.current_y
pose.pose.orientation = self.current_orientation
odom.pose = pose
self.odom_pub.publish(odom)
def create_odometry_message(world_x, world_y, world_theta, world_x_linear_v,
world_y_linear_v, world_z_angular_v, odom_time,
base_frame_id, world_frame_id):
# Convert world orientation (theta) to a Quaternion for use with tf and Odometry
quat_vals = tf.transformations.quaternion_from_euler(0, 0, world_theta)
quat = Quaternion()
quat.x = quat_vals[0]
quat.y = quat_vals[1]
quat.z = quat_vals[2]
quat.w = quat_vals[3]
odom = Odometry()
odom.header.stamp = odom_time
odom.header.frame_id = world_frame_id
odom.pose.pose.position.x = world_x
odom.pose.pose.position.y = world_y
odom.pose.pose.position.z = 0.0 # Because this robot can't fly to a vertical position
odom.pose.pose.orientation = quat
odom.child_frame_id = base_frame_id
odom.twist.twist.linear.x = world_x_linear_v
odom.twist.twist.linear.y = world_y_linear_v
odom.twist.twist.angular.z = world_z_angular_v
return odom
def publish_odom(self, cur_x, cur_y, cur_theta, vx, vth):
# quat = tf.transformations.quaternion_from_euler(0, 0, cur_theta)
quat = euler2quaternion(0, 0, cur_theta)
current_time = rospy.Time.now()
odom = Odometry()
odom.header.stamp = current_time
odom.header.frame_id = 'odom'
odom.pose.pose.position.x = cur_x
odom.pose.pose.position.y = cur_y
odom.pose.pose.position.z = 0.0
odom.pose.pose.orientation = Quaternion(*quat)
odom.pose.covariance[0] = 0.01
odom.pose.covariance[7] = 0.01
odom.pose.covariance[14] = 99999
odom.pose.covariance[21] = 99999
odom.pose.covariance[28] = 99999
odom.pose.covariance[35] = 0.01
odom.child_frame_id = 'base_link'
odom.twist.twist.linear.x = vx
odom.twist.twist.linear.y = 0
odom.twist.twist.angular.z = vth
odom.twist.covariance = odom.pose.covariance
self.ekf_pub.publish(odom)
def create_odometry(self, stamp, robot, velocity_x, velocity_theta):
odom = Odometry()
odom.header.stamp = stamp
odom.header.frame_id = self._odom_frame_id
q = quaternion_from_euler(0, 0, robot.theta)
#set the position
odom.pose.pose.position.x = robot.x
odom.pose.pose.position.y = robot.y
odom.pose.pose.position.z = 0.0
odom.pose.pose.orientation.x = q[0]
odom.pose.pose.orientation.y = q[1]
odom.pose.pose.orientation.z = q[2]
odom.pose.pose.orientation.w = q[3]
#set the velocity
odom.child_frame_id = self._base_frame_id
odom.twist.twist.linear.x = velocity_x
odom.twist.twist.linear.y = 0.0
odom.twist.twist.linear.z = 0.0
odom.twist.twist.angular.x = 0.0
odom.twist.twist.angular.y = 0.0
odom.twist.twist.angular.z = velocity_theta
return odom
def gxPose2d_to_odom(msg,t,outbag,frame_id,child_frame_id,offset):
'''
turn /gx/RosPose2d topic to /odom topic
change the type of gx_sensor/RosPose2d to nav_msgs/Odometry
/gx/RosPose2d topic message example:
timestamp: 1562430246571000
x: 2.03333210945
y: -0.198542118073
theta: 0.551429688931
linearVelocity: 0.0
angularVelociy: -0.00757710635662
the topic is in base_link coordinate
'''
str_gx=[x.split(":") for x in str(msg).split("\n")]
unit_odom=Odometry()
unit_odom.pose.pose.position=Point(float(str_gx[1][1]),float(str_gx[2][1]),0)
unit_odom.pose.pose.orientation=Quaternion(*tf.transformations.quaternion_from_euler(0,0,float(str_gx[3][1])))
unit_odom.header.stamp=Time.from_sec(gx_msg_stamp(msg).to_sec() + offset)
unit_odom.header.frame_id=frame_id
unit_odom.child_frame_id=child_frame_id
unit_odom.twist.twist.angular=Point(0,0,float(str_gx[5][1]))
unit_odom.twist.twist.linear=Point(float(str_gx[4][1])*math.cos(float(str_gx[3][1])),float(str_gx[4][1])*math.sin(float(str_gx[3][1])),0)
outbag.write('/odom',unit_odom,t)
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 parse_line(line):
odom_data = json.loads(line)
if not odom_data:
return None
msg = Odometry()
msg.header.stamp.secs = int(odom_data["secs"])
msg.header.stamp.nsecs = int(odom_data["tf_nsecs"])
msg.header.frame_id = "map"
msg.child_frame_id = "map"
msg.twist.twist.linear.x = float(odom_data["v"])
msg.twist.twist.angular.z = float(odom_data["w"])
msg.pose.pose.position = Point(float(odom_data["px"]),
float(odom_data["py"]),
float(odom_data["pz"]))
msg.pose.pose.orientation = Quaternion(float(odom_data["ox"]),
float(odom_data["oy"]),
float(odom_data["oz"]),
float(odom_data["ow"]))
pose = PoseStamped()
pose.header = msg.header
pose.pose = msg.pose.pose
return msg, pose
def timer_callback(self, event):
if self.last_recieved_stamp is None:
return
if self.flag_reading is True:
return
cmd = Odometry()
cmd.header.stamp = self.last_recieved_stamp
cmd.header.frame_id = self.odom_frame
cmd.child_frame_id = self.base_frame
cmd.pose.pose = self.last_received_pose
cmd.twist.twist = self.last_received_twist
self.odomPublisher.publish(cmd)
if self.publish_tf:
tf = TransformStamped(header=Header(frame_id=cmd.header.frame_id,
stamp=cmd.header.stamp),
child_frame_id=cmd.child_frame_id,
transform=Transform(
translation=cmd.pose.pose.position,
rotation=cmd.pose.pose.orientation))
self.tfBroadcaster.sendTransform(tf)
def publish_odometry(self, translation, quaternion, child, parent):
odom = Odometry()
odom.header.stamp = rospy.Time.now()
odom.header.frame_id = parent
odom.child_frame_id = child
odom.header.seq = self.seq
odom.pose.pose.position.x = translation[0]
odom.pose.pose.position.y = translation[1]
odom.pose.pose.position.z = translation[2]
quat = Quaternion()
quat.x = quaternion[0]
quat.y = quaternion[1]
quat.z = quaternion[2]
quat.w = quaternion[3]
odom.pose.pose.orientation = quat
self.pub_odom.publish(odom)
self.seq += 1
def publish_odometry(x, y, z, th, t, prev):
global odom_pub
odom = Odometry()
time_diff = (t.to_sec() - prev[4].to_sec())
odom.header.frame_id = "odom"
odom.child_frame_id = "base_link"
odom.header.stamp = t
odom.pose.pose.position.x = x
odom.pose.pose.position.y = y
odom.pose.pose.position.z = z
q = tf.transformations.quaternion_from_euler(0, 0, th)
odom.pose.pose.orientation.x = q[0]
odom.pose.pose.orientation.y = q[1]
odom.pose.pose.orientation.z = q[2]
odom.pose.pose.orientation.w = q[3]
odom.twist.twist.linear.x = (x - prev[0]) / time_diff
odom.twist.twist.linear.y = (y - prev[1]) / time_diff
odom.twist.twist.linear.z = (z - prev[2]) / time_diff
odom.twist.twist.angular.z = (th - prev[3]) / time_diff
odom_pub.publish(odom)
def metadata_to_odom(metadata, timestamp, frame_id, child_frame_id):
""" Converts a metadata dictionary to a ROS odometry message.
Args:
metadata: A dictionary containing agent metadata parsed from Unity
AND pre-processed to be converted to the correct frame.
timestamp: A rospy.Time instance for the ROS Odom message instance.
frame_id: A string representing the reference frame (world frame).
Returns:
An Odom ROS message instance that can immediately be published.
"""
odom = Odometry()
odom.header.stamp = timestamp
odom.header.frame_id = frame_id
odom.child_frame_id = child_frame_id
# Pose is in the ENU world frame.
odom.pose.pose.position.x = metadata['position'][0]
odom.pose.pose.position.y = metadata['position'][1]
odom.pose.pose.position.z = metadata['position'][2]
odom.pose.pose.orientation.x = metadata['quaternion'][0]
odom.pose.pose.orientation.y = metadata['quaternion'][1]
odom.pose.pose.orientation.z = metadata['quaternion'][2]
odom.pose.pose.orientation.w = metadata['quaternion'][3]
# Twist is in the body frame (camera/imu).
odom.twist.twist.linear.x = metadata['velocity'][0]
odom.twist.twist.linear.y = metadata['velocity'][1]
odom.twist.twist.linear.z = metadata['velocity'][2]
odom.twist.twist.angular.x = metadata['ang_vel'][0]
odom.twist.twist.angular.y = metadata['ang_vel'][1]
odom.twist.twist.angular.z = metadata['ang_vel'][2]
return odom
def write_gt(gt, i, bag):
utime = gt[i, 0]
x = gt[i, 1]
y = gt[i, 2]
z = gt[i, 3]
euler_roll_rad = gt[i, 4]
euler_pitch_rad = gt[i, 5]
euler_yaw_rad = gt[i, 6]
T_GT_BodyZDown = getTransformationFromEulerAnglesRollPitchYawRadXYZMeters(
euler_roll_rad, euler_pitch_rad, euler_yaw_rad, x, y, z)
T_GT_BZup = T_GT_BodyZDown * get_T_BZU_B().inverse()
timestamp = microseconds_to_ros_timestamp(utime)
rospose = Odometry()
rospose.child_frame_id = "Ground-Truth"
rospose.header.stamp = timestamp
rospose.pose.pose.position.x = T_GT_BZup.getPosition()[0]
rospose.pose.pose.position.y = T_GT_BZup.getPosition()[1]
rospose.pose.pose.position.z = T_GT_BZup.getPosition()[2]
rospose.pose.pose.orientation.x = T_GT_BZup.getRotation().x()
rospose.pose.pose.orientation.y = T_GT_BZup.getRotation().y()
rospose.pose.pose.orientation.z = T_GT_BZup.getRotation().z()
rospose.pose.pose.orientation.w = T_GT_BZup.getRotation().w()
rospose.twist.twist.linear.x = 0.0
rospose.twist.twist.linear.y = 0.0
rospose.twist.twist.linear.z = 0.0
rospose.twist.twist.angular.x = 0.0
rospose.twist.twist.angular.y = 0.0
rospose.twist.twist.angular.z = 0.0
bag.write('/ground_truth', rospose, t=timestamp)
def main():
rospy.init_node('odometry_publisher')
coord = Coord(0.0, 0.0, 0.0)
odom_pub = rospy.Publisher("odom", Odometry, queue_size=50)
odom_broadcaster = tf.TransformBroadcaster()
rospy.Subscriber("moves", String, callback, coord)
while not rospy.is_shutdown():
current_time = rospy.Time.now()
# since all odometry is 6DOF we'll need a quaternion created from yaw
odom_quat = tf.transformations.quaternion_from_euler(0, 0, coord.th)
# first, we'll publish the transform over tf
odom_broadcaster.sendTransform((coord.x, coord.y, 0.), odom_quat,
current_time, "base_link", "odom")
print(coord.x)
# next, we'll publish the odometry message over ROS
odom = Odometry()
odom.header.stamp = current_time
odom.header.frame_id = "odom"
# set the position
odom.pose.pose = Pose(Point(coord.x, coord.y, 0.),
Quaternion(*odom_quat))
# set the velocity
odom.child_frame_id = "base_link"
odom.twist.twist = Twist(Vector3(0, 0, 0), Vector3(0, 0,
0)) #Velocities =0
# publish the message
odom_pub.publish(odom)
pass
def publish_transform_pos(self, msg):
if self.indoor:
# Angle in rad
(roll, pitch,
yaw_before) = tf.transformations.euler_from_quaternion([
msg.pose.pose.orientation.x, msg.pose.pose.orientation.y,
msg.pose.pose.orientation.z, msg.pose.pose.orientation.w
])
# Transform the camera data for Rviz
# 468655
# 5264080
# 468598.24
# 5264012.01
x, y, yaw = self.transformFromLastGPS([
msg.pose.pose.position.x, msg.pose.pose.position.y, yaw_before
], [
468655.0 + self.last_robot_x_edit,
5264080.0 + self.last_robot_y_edit
])
# x, y, yaw = self.transformFromLastGPS([msg.pose.pose.position.x,
# msg.pose.pose.position.y, yaw_before], [468655.0+3.0, 5264080.0-1.0])
odom = Odometry()
odom.header.stamp = rospy.Time.now()
odom.header.frame_id = 'odom'
odom.child_frame_id = 'base_link'
odom.pose.pose.position.x = x
odom.pose.pose.position.y = y
odom.pose.pose.position.z = 0
# Don't forget the "Quaternion()" and *
odom.pose.pose.orientation = Quaternion(
*tf.transformations.quaternion_from_euler(0, 0, yaw))
# publisher
self.transform_cameraPos_pub.publish(odom)
def get_relative(self, odometry):
pose = odometry.pose
x = pose.pose.position.x
y = pose.pose.position.y
orientation = pose.pose.orientation
quaternion = PyKDL.Rotation.Quaternion(orientation.x, orientation.y,
orientation.z, orientation.w)
if not self.initial_quaternion:
self.initial_quaternion = quaternion
self.initial_x = x
self.initial_y = x
else:
relative_x = x - self.initial_x
relative_y = y - self.initial_y
relative_quaternion = self.initial_quaternion * quaternion.Inverse(
)
relative_quaternion = relative_quaternion.GetQuaternion()
odometry = Odometry()
header = Header()
header.stamp = rospy.Time.now()
header.frame_id = "odom"
odometry.header = header
odometry.child_frame_id = "base_link"
odometry.pose.pose.position.x = relative_x
odometry.pose.pose.position.y = relative_y
orientation = Quaternion()
orientation.x = relative_quaternion[0]
orientation.y = relative_quaternion[1]
orientation.z = relative_quaternion[2]
orientation.w = relative_quaternion[3]
odometry.pose.pose.orientation = orientation
self.relative_pub.publish(odometry)
def publish_odom(self, cur_x, cur_y, cur_theta, vx, vth):
"""Publish odometry message"""
quat = tf.transformations.quaternion_from_euler(0, 0, cur_theta)
current_time = rospy.Time.now()
# TODO: why was this commented out?
#br = tf.TransformBroadcaster()
#br.sendTransform((cur_x, cur_y, 0),
# tf.transformations.quaternion_from_euler(0, 0, cur_theta),
# current_time,
# "base_link",
# "odom")
odom = Odometry()
odom.header.stamp = current_time
odom.header.frame_id = 'odom'
odom.pose.pose.position.x = cur_x
odom.pose.pose.position.y = cur_y
odom.pose.pose.position.z = 0.0
odom.pose.pose.orientation = Quaternion(*quat)
# TODO: Why are the covariances set at these values? Do they even make sense?
odom.pose.covariance[0] = 0.01
odom.pose.covariance[7] = 0.01
odom.pose.covariance[14] = 99999
odom.pose.covariance[21] = 99999
odom.pose.covariance[28] = 99999
odom.pose.covariance[35] = 0.01
odom.child_frame_id = 'base_link'
odom.twist.twist.linear.x = vx
odom.twist.twist.linear.y = 0
odom.twist.twist.angular.z = vth
odom.twist.covariance = odom.pose.covariance
self.odom_pub.publish(odom)
