Ask Your Question
0

how to make the relationship between velocity and time be linear by python

asked 2019-03-08 00:43:05 -0600

Redhwan gravatar image

updated 2019-03-11 08:32:04 -0600

I created code which making turtlebot 2 following me depend on detecting my face and chose a value of velocity 0.2 m/s

my issue is the movement of the robot when disappearing my face suddenly which making turtlebot stops suddenly, I need to make decreasing its velocity gradually like this figure link text

my experience not good in ROS

I need to change the line self.twist.linear.x = 0.05 by the linear relationship between velocity and time as shown in the figure in the link, I mean that turtlebot will stop after a certain time, for example, 20 second

my full code:

#!/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.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)

        if(type(faces) != tuple):
            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):
            self.twist.linear.x = 0.05
            self.cmd_vel_pub.publish(self.twist)


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

please help me or any suggestion

Thank you in advance

edit retag flag offensive close merge delete

1 Answer

Sort by » oldest newest most voted
1

answered 2019-03-08 02:31:41 -0600

Delb gravatar image

updated 2019-03-11 08:48:34 -0600

You have two ways of doing this :

  • By calculations :

You have to define the acceleration you want (well in your case this is a deceleration). Taking your example, you want to go from 0.2 m/s to 0.05 m/s in 20 seconds meaning you have dv = -0.15 and dt = 20. (dv = vfinal - vinitial that's why it's negative). Since a = dv/dt you know you want a constant acceleration of -0.0075 m/s² (or a deceleration of 0.0075 m/s²).

Now if you integrate the equation of the acceleration and assume the acceleration is constant you get v = a * t + K (K is another constant from the integration). You know v(t=0)=0.2 so you have v = a * 0 + K = 0.2 => K = 0.2. You also know that a = -0.0075 so that's it you have the linear relationship between velocity and time :

v = -0.0075 * t + 0.2
  • Another option is to use a velocity smoother which basically do all those calculations for you. You can set limits for the acceleration and velocity. There is a package compatible with the turtlebot (with the mobile base kobuki to be accurate) : yocs_velocity_smoother. You will need to use a launch file with a nodelet manager and define a yaml file to set your parameters and then call the nodelet. I can give you a launch file example if you want.

Edit :

I tried to solve by the first way, my issue is in how to find (t,t0) in python with rospy because of lacking my experience in ROS

You will need to save the starting time when you start decelerating and then just substract the current time to get the value for t. It can be done like that :

#global variable outside your callback
starting_time = rospy.get_rostime().to_sec()
save_time = True 
...
if(save_time == True):
    save_time = False  #Condition only the first time
    starting_time = rospy.get_rostime().to_sec() #save the current time
now = rospy.get_rostime().to_sec()
t = now - starting_time           
self.twist.linear.x = -0.0075 * t + 0.2
self.cmd_vel_pub.publish(self.twist)

For the second method you can go to the wiki I've linked and select the indigo version, I just saw that there is no documentation for the melodic distribution. Moreover you can see an example by looking at standalone.launch and check the standalone.yaml file too. This package allows you to set speed limits and acceleration limits so with your example you don't have to change your code, just to use the velocity_smoother_nodelet and set the parameter accel_lim_v to 0.0075.

Edit 2:

I need to recount from zero when lost my face

You just need to reset your flag save_time when you don't see the face anymore with an else condition for example :

if(type(faces) == tuple):
    if(save_time == True):
        save_time = False  #Condition only the first time
        starting_time = rospy.get_rostime().to_sec() #save the current time
    now = rospy.get_rostime().to_sec()
    t = now - starting_time           
    self.twist.linear.x ...
(more)
edit flag offensive delete link more

Comments

first of all, thank you so much for your answer

I tried to solve by the first way, my issue is in how to find (t,t0) in python with rospy because of lacking my experience in ROS

I am confusing, I tried to understand it from this code ( http://wiki.ros.org/turtlesim/Tutoria... ) but failed

the second way is ambiguous for me

Redhwan gravatar imageRedhwan ( 2019-03-08 05:15:36 -0600 )edit

Thank you so much to help me, the output after 27 second and 5.3 m which assume that the speed becomes zero: 0.199999976754, ..., ..., ..., 0.199999955297

Redhwan gravatar imageRedhwan ( 2019-03-08 07:18:17 -0600 )edit

What are those numbers ? To make it clear you should edit your question to add your new code.

Delb gravatar imageDelb ( 2019-03-08 08:02:31 -0600 )edit

sorry, if I make any mistake in my question

those number is vfinal to my turtlebot 2 after 27 sec

to clear:

my question after 20 sec ....> vfinal =0.05 m/s also in 0 sec ....> vfinal =0.2 m / s=vinitial as you mention above

this my figure link text

Redhwan gravatar imageRedhwan ( 2019-03-08 08:19:44 -0600 )edit

I can't help without seeing how you got those results. Please provide your code.

Delb gravatar imageDelb ( 2019-03-08 08:47:01 -0600 )edit

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):
        starting_time = rospy.get_rostime().to_sec()
        save_time = True
        if(save_time == True):
            save_time = False  #Condition only the first time
            starting_time = rospy.get_rostime().to_sec() #save the current time
        now = rospy.get_rostime().to_sec()
        t = now - starting_time           
        v = self.twist.linear.x = -0.0075 * t + 0.2
        print v
        self.cmd_vel_pub.publish(self.twist)
Redhwan gravatar imageRedhwan ( 2019-03-08 08:51:09 -0600 )edit

sorry, I can't write full code in one comment because of the number of characters

rest code the same above

Redhwan gravatar imageRedhwan ( 2019-03-08 08:56:30 -0600 )edit

I am very sorry, my mistake that I put starting_time = rospy.get_rostime().to_sec() & save_time = True inside my callback

The outputs now are good but another issue has emerged, Once the speed reaches zero, then start to increase.

I need to stop it.

thank you @Delb to help me

Redhwan gravatar imageRedhwan ( 2019-03-09 01:02:20 -0600 )edit

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

2 followers

Stats

Asked: 2019-03-08 00:43:05 -0600

Seen: 145 times

Last updated: Mar 11 '19