如何通过ROS-python使速度和时间之间的关系成为线性

Question

我创建的代码使得跟随我的turtlebot 2依赖于检测我的脸并选择速度为0.2 m / s的值。

我的问题是当机器人突然消失时，机器人的运动突然停止，我需要逐渐降低速度，就像这个数字一样enter link description here

我的经历在ROS'time不好

我需要它从零开始计数从零丢失我的脸。我的问题在我的代码中，一旦运行代码，时间会不断增加，无论是否丢失了我的脸。在这一行

 v = self.twist.linear.x = (-0.07 * t + 0.2)

我的完整代码：

#!/usr/bin/env python



import rospy
from sensor_msgs.msg import Image
from geometry_msgs.msg import Twist
import cv2, cv_bridge


face_cascade = cv2.CascadeClassifier('/home/redhwan/1/run-webcam/Face-Detect-Demo-by-Ali-master/haarcascade_frontalface_default.xml' )


class Face_detection:
    def __init__(self):
        self.bridge = cv_bridge.CvBridge()
        self.starting_time = rospy.get_rostime().to_sec()
        self.save_time = True


        self.image_sub = rospy.Subscriber('/usb_cam/image_raw',Image, self.image_callback)
        self.cmd_vel_pub = rospy.Publisher('/cmd_vel_mux/input/teleop',Twist, queue_size=1)
        self.twist = Twist()


    def image_callback(self, msg):
        image = self.bridge.imgmsg_to_cv2(msg,desired_encoding='bgr8')

        gray = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY)
        faces = face_cascade.detectMultiScale( gray,scaleFactor=1.1,minNeighbors=5,minSize=(30, 30),flags=cv2.cv2.CASCADE_SCALE_IMAGE)
        faces = face_cascade.detectMultiScale(gray, 1.3, 5)

        for (x, y, w, h) in faces:
            cv2.rectangle(image, (x, y), (x+w, y+h), (0, 255, 0), 2)


            self.twist.linear.x = 0.2

            self.cmd_vel_pub.publish(self.twist)


        cv2.imshow('face ', image)


        cv2.waitKey(3)




        if(type(faces) == tuple):

            if(self.save_time == False):
#                self.save_time = False  #Condition only the first time
                self.starting_time = rospy.get_rostime().to_sec() #save the current time
            now = rospy.get_rostime().to_sec()
#                self.save_time == False
            t = (now - self.starting_time)
            print ('t',t)
            if t <2.9:

                v = self.twist.linear.x = (-0.07 * t + 0.2)
                print v
                self.cmd_vel_pub.publish(self.twist)


            if t >= 2.9:
                v = self.twist.linear.x = 0
                print v
                self.cmd_vel_pub.publish(self.twist)


rospy.init_node('face_detection')
follower = Face_detection()
rospy.spin()

请帮我

预先感谢

Answer 1

如果您只需要这样做，就可以使斑鸠的运动更加顺畅。您可能会发现velocity smoother package将满足您的需求。您可以通过运行来安装它：sudo apt install ros-kinetic-yocs-velocity-smoother节点接受原始速度输入并根据加速度参数对其进行过滤。因此，您可以将cmd_velocity_mux输出重新映射到raw_cmd_vel，并将平滑后的输出smooth_cmd_vel重新映射到输入到turilerot的输入。