vision_to_mavros
vision_to_mavros copied to clipboard
VISION_POSITIONZ_ESTIMATE does now show odometry data
Hello guys thank you for such amazing discussion.
I have been using t265 but since it is broke I had to change to the zed mini.
I have implemented a scrip to publish the pose and pose with covariance to mavros/vision_pose/...
In the mavlink inspector I can only see VISION_POSITION_ESTIMATE values but before when I used the t265 mavlink inspector it was showing odometry data pose and orientation quaternion.
Is it okay to just receive VISION_POSITION_ESTIMATE x,y,z,roll,pitch, and yaw?
or What should I do?
This is my code
import rospy
import mavros
from nav_msgs.msg import Odometry
from geometry_msgs.msg import PoseStamped
from geometry_msgs.msg import PoseWithCovarianceStamped
mavros.set_namespace()
def zed_convertion(msg):
pose = PoseStamped()
pose_cov = PoseWithCovarianceStamped()
# Pose with covariance
pose_cov.header = msg.header
pose_cov.pose = msg.pose
# Pose without covariance
pose.header = msg.header
pose.pose = msg.pose.pose
rospy.loginfo(pose)
pose_pub.publish(pose)
pose_cov_pub.publish(pose_cov)
def main_converter():
rospy.init_node('zed_pose_converter', anonymous=True)
rospy.Subscriber("/zedm/zed_node/odom", Odometry, zed_convertion)
rate = rospy.Rate(10) # 10hz
while not rospy.is_shutdown():
rate.sleep()
if __name__ == '__main__':
try:
pose_pub = rospy.Publisher('/mavros/vision_pose/pose', PoseStamped, queue_size=10)
pose_cov_pub = rospy.Publisher('/mavros/vision_pose/pose_cov', PoseWithCovarianceStamped, queue_size=10)
main_converter()
except rospy.ROSInterruptException:
pass