vision_to_mavros icon indicating copy to clipboard operation
vision_to_mavros copied to clipboard

VISION_POSITIONZ_ESTIMATE does now show odometry data

Open DiegoHerrera1890 opened this issue 2 years ago • 0 comments

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? zed_camera_qgc_mavlink 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

DiegoHerrera1890 avatar Sep 29 '22 05:09 DiegoHerrera1890