How are you? How can I turn turtlebot2 10 degrees about its axis and then pause.
Note: The robot will not move, it will just rotate 10 degrees once every say 20 seconds.
I tried setting the Twist() object's angular velocity in the Z direction but only one wheel moves and ultimately the robot is displaced.
below is my code:
angular_velocity = radians(10) #10 degs / s
r = rospy.Rate(5) #5Hz
#rospy.loginfo("starting loop")
while not rospy.is_shutdown():
rospy.loginfo("in loop")
new_angle = (angle + new_angle) % 360
real_angle = real_angle + angle
#real_angle = rea1_angle + 5
rospy.loginfo("after addition")
new_angle = real_angle % 360
turn_cmd.angular.z = angular_velocity #turns 10 degrees a second
#turn at 10 degrees a second for 1 second
for i in range(5):
self.cmd_vel.publish(turn_cmd)
r.sleep()
r.sleep()
# wait 40s
for x in range(200):
self.cmd_vel.publish(Twist()) #publish ) to stop the bot at that point
# stop the turtle and gather data as u wait
r.sleep()
try:
data = "get some data from external source"
if(data):
try:
#rospy.loginfo("w(rads/$
comb_d.write( str(data)
except:
rospy.loginfo("failed or bad data)
pass
r.sleep()
comb_d.close()
def shutdown(self):
# stop turtlebot
rospy.loginfo("Stop turning")
self.cmd_vel.publish(Twist())
The above is my logic. But not only does the robot move, it also does not turn 10 degrees as I expect.
What am i doing wrong?
Thank you very much.
Am using ROS indigo kobuki and turtlebot2
That being said - on slightly different note - what would be the favorable values for the rospy.Rate() function and the inner for loop to achieve the 10 or even 30 degree rotation at a relative higher angular velocity, say 25 degrees per second. Because it seems the robot does not work well with low velocities such as 10 degs/s. Some times it does not move at all when low velocities are given. Eitherway, thanks.
Related
i am working on a project of swarm algorithms and i am trying to make complex shapes using the swarm consensus. However, the mathematics to achieve that seems quite difficult for me.
I have been able to make shapes like stars, circle and triangle but to make other complex shapes seems more harder. It would be very helpful if i get the idea of using numpy arrays to build these complex shapes using swarms....................................................
# general function to reset radian angle to [-pi, pi)
def reset_radian(radian):
while radian >= math.pi:
radian = radian - 2*math.pi
while radian < -math.pi:
radian = radian + 2*math.pi
return radian
# general function to calculate next position node along a heading direction
def cal_next_node(node_poses, index_curr, heading_angle, rep_times):
for _ in range(rep_times):
index_next = index_curr + 1
x = node_poses[index_curr][0] + 1.0*math.cos(heading_angle)
y = node_poses[index_curr][1] + 1.0*math.sin(heading_angle)
node_poses[index_next] = np.array([x,y])
index_curr = index_next
return index_next
##### script to generate star #####
filename = 'star'
swarm_size = 30
node_poses = np.zeros((swarm_size, 2))
outer_angle = 2*math.pi / 5.0
devia_right = outer_angle
devia_left = 2*outer_angle
# first node is at bottom left corner
heading_angle = outer_angle / 2.0 # current heading
heading_dir = 0 # current heading direction: 0 for left, 1 for right
seg_count = 0 # current segment count
for i in range(1,swarm_size):
node_poses[i] = (node_poses[i-1] +
np.array([math.cos(heading_angle), math.sin(heading_angle)]))
seg_count = seg_count + 1
if seg_count == 3:
seg_count = 0
if heading_dir == 0:
heading_angle = reset_radian(heading_angle - devia_right)
heading_dir = 1
else:
heading_angle = reset_radian(heading_angle + devia_left)
heading_dir = 0
print(node_poses)
with open(filename, 'w') as f:
pickle.dump(node_poses, f)
pygame.init()
# find the right world and screen sizes
x_max, y_max = np.max(node_poses, axis=0)
x_min, y_min = np.min(node_poses, axis=0)
pixel_per_length = 30
world_size = (x_max - x_min + 2.0, y_max - y_min + 2.0)
screen_size = (int(world_size[0])*pixel_per_length, int(world_size[1])*pixel_per_length)
# convert node poses in the world to disp poses on screen
def cal_disp_poses():
poses_temp = np.zeros((swarm_size, 2))
# shift the loop to the middle of the world
middle = np.array([(x_max+x_min)/2.0, (y_max+y_min)/2.0])
for i in range(swarm_size):
poses_temp[i] = (node_poses[i] - middle +
np.array([world_size[0]/2.0, world_size[1]/2.0]))
# convert to display coordinates
poses_temp[:,0] = poses_temp[:,0] / world_size[0]
poses_temp[:,0] = poses_temp[:,0] * screen_size[0]
poses_temp[:,1] = poses_temp[:,1] / world_size[1]
poses_temp[:,1] = 1.0 - poses_temp[:,1]
poses_temp[:,1] = poses_temp[:,1] * screen_size[1]
return poses_temp.astype(int)
disp_poses = cal_disp_poses()
# draw the loop shape on pygame window
color_white = (255,255,255)
color_black = (0,0,0)
screen = pygame.display.set_mode(screen_size)
screen.fill(color_white)
for i in range(swarm_size):
pygame.draw.circle(screen, color_black, disp_poses[i], 5, 0)
for i in range(swarm_size-1):
pygame.draw.line(screen, color_black, disp_poses[i], disp_poses[i+1],2)
pygame.draw.line(screen, color_black, disp_poses[0], disp_poses[swarm_size-1], 2)
pygame.display.update()
Your method for drawing takes huge advantage of the symmetries in the shapes you are drawing. More complex shapes will have fewer symmetries and so your method will require a lot of tedious work to get them drawn with stars. Without symmetry you may be better served writing each individual line 'command' in a list and following that list. For example, drawing the number 4 starting from the bottom (assuming 0 degrees is --> that way):
angles = [90,225,0]
distances = [20,15,12]
Then with a similar program to what you have, you can start drawing dots in a line at 90 degrees for 20 dots, then 225 degrees for 15 dots etc... Then by adding to these two lists you can build up a very complicated shape without relying on symmetry.
I'm wondering the best way to simulate a sunrise/sunset on the y and z axis. At the moment it does what I want but is moving too fast (say every 3 seconds it's already completed an entire sun path and is already moving in reverse back to the origin).
I know this has to do with seconds variable combined with sin and cos, as this function is called and moves the light's position every frame. Basically I want it to be linked to my game's timer of 50:
50 seconds it's sunrise
25 seconds it's noon
0 seconds it's sunset/game over
Currently I have:
lightPosition = Point_E3d(0,
std::abs(100 *std::cos(seconds)),
-100 * std::sin(seconds));
Wondering what's the correct code format to achieve this effect.
This is just simple trigonometry. The period (Time until the function repeats) of sine(x * n) and cosine(x * n) are both 2*pi / n. In this case, n = 1, so the period is 6.28, meaning one full day (and night) will last 6.28 seconds. If you want to increase the period, multiply your seconds argument by a number smaller than one. A little bit of algebra shows that:
period of sin(x * n) = 2*pi / n
period of sin(.1256 * x) = 2*pi / .1256 = 6.28 / 0.1256 = 50
Therefore, take sine and cosine of seconds * 0.1256, rather than just seconds.
lightPosition = Point_E3d(0,
std::abs(100 *std::cos(seconds * 0.1256 )),
-100 * std::sin(seconds * 0.1256));
Note that this is also including the night time. If you want just the 12 hour day time period to last 50 seconds, multiply by half of 0.1256, aka 0.0628.
I am working in vehicle counting with opencv and python programming, I already complete step:
1. Detect moving vehicle with BackgroundSubtractorMOG2
2. Draw rectangle on it, then poin a centroid of that
3. Draw a line (to indicate of the counting)
if that centroid accros/intercept with the line I want count that 1. but in my code sometime it add sometime no. Here the line code:
cv2.line(frame,(0,170),(300,170),(200,200,0),2)
and there the centroid:
if w > 20 and h > 25:
cv2.rectangle(frame, (x,y), (x+w,y+h), (180, 1, 0), 1)
x1=w/2
y1=h/2
cx=x+x1
cy=y+y1
centroid=(cx,cy)
cv2.circle(frame,(int(cx),int(cy)),4,(0,255,0),-1)
my counting code:
if cy==170:
counter=counter+1
Can anyone help me. please. for your advice thankyou!
Here is my approach that would work independently of the video frame rate. Assuming that you are able to track a car's centroid at each frame, I would save the last two centroids' position (last_centroid and centroid in my code) and process as follows:
compute the intercepting line equation's parameters ( (a,b,c) from aX + bY + c = 0)
compute the equation's parameters of the segment line between last_centroid and centroid
find if the two lines are intersecting
if so, increment your counter
Here is how I implemented it in OpenCV (Python):
import cv2
import numpy as np
import collections
Params = collections.namedtuple('Params', ['a','b','c']) #to store equation of a line
def calcParams(point1, point2): #line's equation Params computation
if point2[1] - point1[1] == 0:
a = 0
b = -1.0
elif point2[0] - point1[0] == 0:
a = -1.0
b = 0
else:
a = (point2[1] - point1[1]) / (point2[0] - point1[0])
b = -1.0
c = (-a * point1[0]) - b * point1[1]
return Params(a,b,c)
def areLinesIntersecting(params1, params2, point1, point2):
det = params1.a * params2.b - params2.a * params1.b
if det == 0:
return False #lines are parallel
else:
x = (params2.b * -params1.c - params1.b * -params2.c)/det
y = (params1.a * -params2.c - params2.a * -params1.c)/det
if x <= max(point1[0],point2[0]) and x >= min(point1[0],point2[0]) and y <= max(point1[1],point2[1]) and y >= min(point1[1],point2[1]):
print("intersecting in:", x,y)
cv2.circle(frame,(int(x),int(y)),4,(0,0,255), -1) #intersecting point
return True #lines are intersecting inside the line segment
else:
return False #lines are intersecting but outside of the line segment
cv2.namedWindow('frame')
frame = np.zeros((240,320,3), np.uint8)
last_centroid = (200,200) #centroid of a car at t-1
centroid = (210,180) #centroid of a car at t
line_params = calcParams(last_centroid, centroid)
intercept_line_params = calcParams((0,170), (300,170))
print("Params:", line_params.a,line_params.b,line_params.c)
while(1):
cv2.circle(frame,last_centroid,4,(0,255,0), -1) #last_centroid
cv2.circle(frame,centroid,4,(0,255,0), -1) #current centroid
cv2.line(frame,last_centroid,centroid,(0,0,255),1) #segment line between car centroid at t-1 and t
cv2.line(frame,(0,170),(300,170),(200,200,0),2) #intercepting line
print("AreLinesIntersecting: ",areLinesIntersecting(intercept_line_params,line_params,last_centroid,centroid))
cv2.imshow('frame',frame)
if cv2.waitKey(15) & 0xFF == ord('q'):
break
cv2.destroyAllWindows()
And here are some results:
Fig1. Segment is intersecting the line (intercepting line in blue - segment line between last_centroid and centroid in red)
Fig2. Segment is NOT intersecting the line
N.B. I found the formulas to calculate the intersection point here.
I hope my approach will help to address your problem.
To assume that the centroid will assume a position 170 (in x or y) is wrong, because videos generally works at 30 fps, that mean you will get 30 centroid locations per second which means even if there the object crosses the line, it may never be 170!
To counter this, one method that can be used is defining a line margin. This means now you have a line margin x before actual line (y = 170) and x after the line margin.
So if your object falls anywhere in the margin, you can increment the counter. Now the next big part would be to make a tracking mechanism wherein you collect the list of point for each object and check if it fell in the margin.
I am trying to draw a clock face in which the second hand moves as the time changes.
I am using the following to calculate the points on my circle:
def points_on_circle():
global time, radius, centre, x, y
theta = time% math.pi * 2
c = math.cos(theta)
s = math.sin(theta)
x= centre[0] + radius * c
y =centre[1] + radius * s
return x,y
my timer 'ticks' every tenth second, the radius of my circle is 50, the centre is at (150,150) which is also the origin of my hand, the other end of the hand being (x,y) as calculated above. How do I calculate , I assume by multiplying time by some constant, how fast the x,y should change for this circle ( but for any circle) .
I am using CodeSkulptor from Coursera to try to do this ( the original assignment created a digital timer which I am done. This is not part of the homework ( yet??)
The hand rotates 2 Pi radians every 60 seconds. Assuming you're syncing with real time, time.time() will return the current time in seconds (and milliseconds which I suggest you ignore). If you take the time and first do numseconds = int(time.time()) % 60, you now need to translate that, which is a simple as numseconds * 2 * pi / 60. (Example: numseconds = 0, theta = 0, numseconds = 15, theta = pi /2 (90 degrees).
You will also need to play with your equations as normally theta=0 implies the line is horizontal pointing right (which would be numseconds = 15) and theta=Pi implies the line is vertical pointing up (which would be numseconds = 0)
I'm having a trouble with making my Tank accelerate when I press a move button and decelerate when I let go of it. Don't be too harsh on me for not being a pro because I'm still learnig and Thanks in advance!
I also have variables called 'speed' and 'maxspeed' which I played around with and it didn't turn out to well, and the code below basicaly makes my tank move in an update function
if(TestName->Tank[0].up == true){
TestName->Tank[0].position_y = TestName->Tank[0].position_y + (TestName->Tank[0].speed + 0.06f);
}
if(TestName->Tank[0].down == true){
TestName->Tank[0].position_y = TestName->Tank[0].position_y - 0.06f;
}
if(TestName->Tank[0].right == true){
TestName->Tank[0].rotation = TestName->Tank[0].rotation + 0.6f;
}
if(TestName->Tank[0].left == true){
TestName->Tank[0].rotation = TestName->Tank[0].rotation - 0.6f;
}
}
In this bit of code:
TestName->Tank[0].position_y = TestName->Tank[0].position_y + (TestName->Tank[0].speed + 0.06f);
it looks horribly like you are trying to add an acceleration (0.06) to your speed while at the same time adding the speed to the position. The plus sign won't change the speed, it will just calculate the result and use it in the equation.
Do it in two steps:
TestName->Tank[0].speed += 0.06f;
TestName->Tank[0].position_y += TankName->Tank[0].speed;
When you have that bit working your next issue will be inability to steer. I think you would be well advised to google vectors (maths, not C++) and the difference between velocity and speed.
Logically,.....Supposing your defaultspeed is 1 and maxspeed is 2. You probably want another variable, lets call it deltaSpeed.
Then your totalSpeed(Change in Tank's Position in a direction) is defaultspeed * deltaSpeed.
You would also need to check if the user is pressing down on the button, if so increase(either by a fixed amount or exponentially) the deltaSpeed. When the user release the button then decrease the deltaSpeed. This would give the illusion of tank speeding up and slowing down.
Then, when the user presses another direction you could reset the deltaSpeed.
some useful Newtonian equations:
f = ma
v^2 = u^2 + 2as
s = ut + 0.5 * a * t^2
where:
s = distance travelled (m)
u = initial velocity (m/s)
t = time (seconds)
a = acceleration
f = force
m = mass
Rearranging these equations allows you to compute the change in acceleration, and therefore velocity, and therefore distance for a given time period.
Games will model these rules with varying degrees of simplification (e.g. you could code that pressing 'W' will generate a Force of X Newtons in the direction the tank is facing, or you could choose to limit the maximum speed, for make force inversely proportional to current speed (to model for the power equation).
Or you could calculate F as the integral of the power equation over the time period between each simulation pass (which is what a flight sim might do).
Hopefully some ideas to guide you to some useful literature.