msg.twist.twist.linear.z = 0
odom_pub.publish(msg)
# br = tf.TransformBroadcaster()
# br.sendTransform((msg.pose.pose.position.x, msg.pose.pose.position.y, msg.pose.pose.position.z),
# q,
# rospy.Time.now(),
# "base_link",
# "world")
# br = tf.TransformBroadcaster()
# e = tfs.euler_from_quaternion(q, 'rzyx')
# qc = tfs.quaternion_from_euler(e[0], 0.0, 0.0, 'rzyx')
# br.sendTransform((msg.pose.pose.position.x, msg.pose.pose.position.y, msg.pose.pose.position.z),
# qc,
# rospy.Time.now(),
# "intermediate",
# "world")
msg = PositionCommand()
msg.header.stamp = rospy.Time.now()
msg.header.frame_id = "world"
msg.position.x = 0.2
msg.position.y = 0.2
msg.position.z = 1.2
msg.trajectory_flag = msg.TRAJECTORY_STATUS_READY
msg.trajectory_id = count // 2 + 1
pos_pub.publish(msg)
count += 1
