使/mavros/imu/data_raw话题发出来而且话题频率在200hz左右,给vins订阅使用

启动px4.launch之后有时会遇到imu话题只有/mavros/imu/data发出来,而且话题频率只有15hz,/mavros/imu/data_raw话题没有发出来,可以通过下面命令使得/mavros/imu/data_raw话题发出来而且话题频率在200hz左右。这么做主要是给vins使用。但是/mavros/imu/data话题还是在15hz,这个不影响,vins是订阅/mavros/imu/data_raw话题。下面这个命令飞控重启后就会失效,需要重新运行。

rosrun mavros mavcmd long 511 105 5000 0 0 0 0 0

  • 2
    点赞
  • 6
    收藏
    觉得还不错? 一键收藏
  • 0
    评论
以下是一个简单的ROS节点示例,用于使用KCF算法跟踪小车并发布位置和速度指令到MAVROS。 ```python #!/usr/bin/env python import rospy from sensor_msgs.msg import Image from cv_bridge import CvBridge import cv2 import numpy as np from pyimagesearch.centroidtracker import CentroidTracker from pyimagesearch.trackableobject import TrackableObject from geometry_msgs.msg import PoseStamped, Twist class KCFTrackerNode: def __init__(self): rospy.init_node('kcf_tracker_node', anonymous=True) self.bridge = CvBridge() self.ct = CentroidTracker() self.trackers = [] self.trackable_objects = {} self.image_sub = rospy.Subscriber('/iris/usb_cam/image_raw', Image, self.image_callback) self.position_pub = rospy.Publisher('/mavros/setpoint_position/local', PoseStamped, queue_size=1) self.velocity_pub = rospy.Publisher('/mavros/setpoint_velocity/cmd_vel_unstamped', Twist, queue_size=1) self.image_width = 640 self.image_height = 480 self.focal_length = 600 self.real_width = 0.5 self.target_width = 0.1 def image_callback(self, data): cv_image = self.bridge.imgmsg_to_cv2(data, 'bgr8') if len(self.trackers) == 0: # initialize trackers objects = self.ct.update([(0, 0, self.image_width, self.image_height)]) for (object_id, centroid) in objects.items(): tracker = cv2.TrackerKCF_create() tracker.init(cv_image, (centroid[0], centroid[1], self.target_width * self.focal_length, self.target_width * self.focal_length)) self.trackers.append(tracker) self.trackable_objects[object_id] = TrackableObject(object_id, centroid) else: # update trackers for tracker in self.trackers: success, box = tracker.update(cv_image) if success: (x, y, w, h) = [int(v) for v in box] centroid = (x + w / 2, y + h / 2) object_id = self.ct.register(centroid) to = self.trackable_objects.get(object_id, None) if to is None: to = TrackableObject(object_id, centroid) self.trackable_objects[object_id] = to else: to.centroids.append(centroid) # filter out small objects self.trackable_objects = {k: v for k, v in self.trackable_objects.items() if len(v.centroids) > 5 and v.width() > self.image_width * 0.1} # update position and velocity commands for object_id, to in self.trackable_objects.items(): x = (to.centroids[-1][0] - self.image_width / 2) * self.real_width / self.focal_length y = (to.centroids[-1][1] - self.image_height / 2) * self.real_width / self.focal_length z = 2.0 pose_msg = PoseStamped() pose_msg.header.stamp = rospy.Time.now() pose_msg.pose.position.x = x pose_msg.pose.position.y = y pose_msg.pose.position.z = z self.position_pub.publish(pose_msg) vx = (to.centroids[-1][0] - to.centroids[-2][0]) * self.real_width / self.focal_length vy = (to.centroids[-1][1] - to.centroids[-2][1]) * self.real_width / self.focal_length vz = 0.0 vel_msg = Twist() vel_msg.linear.x = vx vel_msg.linear.y = vy vel_msg.linear.z = vz self.velocity_pub.publish(vel_msg) if __name__ == '__main__': try: node = KCFTrackerNode() rospy.spin() except rospy.ROSInterruptException: pass ``` 请注意,此节点使用了pyimagesearch库中的CentroidTracker和TrackableObject类,您需要先安装此库: ```bash pip install imutils ``` 此外,这里的代码将图像中间作为目标点,将图像宽度的10%用作最小目标宽度,将真实世界中的实际宽度设置为0.5米。您可能需要根据您的具体应用程序进行一些修改。
评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值