if __name__ == '__main__':
rospy.init_node('gray')
settings = termios.tcgetattr(sys.stdin)
pub = rospy.Publisher('cmd_vel', Twist, queue_size=1)
x = 0
th = 0
node = Gray()
node.main()
We make the publisher(cmd_vel) in main, and run the main function of class gray.
def __init__(self):
self.r = rospy.Rate(10)
self.selecting_sub_image = "compressed" # you can choose image type "compressed", "raw"
if self.selecting_sub_image == "compressed":
self._sub = rospy.Subscriber('/raspicam_node/image/compressed', CompressedImage, self.callback, queue_size=1)
else:
self._sub = rospy.Subscriber('/usb_cam/image_raw', Image, self.callback, queue_size=1)
self.bridge = CvBridge()
init function makes a subscriber, which runs 'callback' when it gets data.
def main(self):
rospy.spin()
Then it runs the spin() function.
v, ang = vel_select(lvalue, rvalue, left_angle_num, right_angle_num, left_down, red_dots)
self.sendv(v, ang)
Inside the callback function, it gets a linear speed and angular speed value, and runs a sendv function to send it to the subscribers.
def sendv(self, lin_v, ang_v):
twist = Twist()
speed = rospy.get_param("~speed", 0.5)
turn = rospy.get_param("~turn", 1.0)
twist.linear.x = lin_v * speed
twist.angular.z = ang_v * turn
twist.linear.y, twist.linear.z, twist.angular.x, twist.angular.y = 0, 0, 0, 0
pub.publish(twist)
and... sendv function sends it to the turtlebot.
It has to move continuously, because if we do not publish data, it still has to move with the speed it got from the last publish. Also, callback function runs every 0.1 seconds, so it keeps sending data.
But it does not move continously. It stops for a few seconds, and go for a very short time, and stops again, and go for a very short time, and so on. The code which selects the speed works correctly, but the code who sents it to the turtlebot does not work well. Can anyone help?
Also, callback function runs every 0.1 seconds.
I believe this is incorrect. I see that you have made a self.r object but never used it anywhere in the code to achieve an update rate of 10hz. If you want to run the main loop at every 0.1 seconds, you will have to call your commands within the following loop (see rospy-rates) before calling rospy.spin():
self.r = rospy.Rate(10)
while not rospy.is_shutdown():
<user commands>
self.r.sleep()
However, this would not help you either since your code is publishing to /cmd_vel within the subscriber callback which gets called only on receiving some data from the subscriber. So basically, your /cmd_vel is not being published at the rate of 10hz but rather at the rate at which you are receiving the data from the subscribed topic ('/raspicam_node/image/compressed'). Since these are image topics, they might be taking a lot of time to be updated hence the delay in your velocity commands to the robot.
Related
I have a program with 2 timer functions like this (p_maj_input and p_maj_output_motion are equal so the period of both timer is the same):
maj_input = timer('ExecutionMode','fixedRate','Period',p_maj_input,'TasksToExecute', ...
floor(Tmeasurement/p_maj_input)-1,'TimerFcn',{#time_append,p_maj_input,HR_all,freq_HR, ...
BVP_all,freq_BVP,TEMP_all,freq_TEMP,ACC_x_all,ACC_y_all,ACC_z_all,freq_ACC,EDA_all,freq_EDA, ...
folder_all,all_dir,num_dir}); start(maj_input);
maj_output_motion=timer('ExecutionMode','fixedRate','Period',p_maj_output_motion,'TasksToExecute', ...
floor(Tmeasurement/p_maj_output_motion)-1,'TimerFcn',{#output_motion_append, ...
freq_MOTION,freq_mvt_score,freq_walk,p_maj_output_motion,p_maj_output_walk,folder_all,all_dir,num_dir});%,'StartDelay',min(5,p_maj_output_motion)); %startDelay must be min 5 for walk detection start(maj_output_motion);
In each timer callback function there is a loop over subfolders contained in a folder that is selected at the beginning of the program.
output_motion_append(obj,event,freq_MOTION,freq_mvt_score,freq_walk,p_maj_output_motion,p_maj_output_walk,folder_all,all_dir,num_dir)
fprintf('motion %d\n',obj.TasksExecuted)
toc
for folder_index=1:num_dir
[folder_original,folder_fictive] = subfolderFunction(all_dir, folder_all, folder_index);
fileName=strcat(folder_fictive,'\ACC.csv');
[ACC_time, ACC_x, ACC_y, ACC_z] = loadValuesE4_acc(fileName);
% Motion Amount
[agitation,agitation_ts] = identifyAgitation(ACC_x,ACC_y,ACC_z,ACC_time);
agitation=agitation';
len1=length(agitation);
if len1<=freq_MOTION*p_maj_output_motion
i_init1=1;
elseif len1>freq_MOTION*p_maj_output_motion
i_init1=len1-freq_MOTION*p_maj_output_motion+1;
end
writematrix([agitation(i_init1:len1)],strcat(folder_fictive,'\MOTION_output.csv'),'WriteMode','Append');
writematrix([mvt_score(i_init2:len2)],strcat(folder_fictive,'\neurologicScore_output.csv'),'WriteMode','Append');
end
end
Everything works fine if the number of subfolders is lower than 4 : the good values appear in the files on which is carried out the writematrix function. And the timer callback function are are called one after the other, so both timer work simultaneously.
However if there are 5 subfolders or more, it is not the good values and using the debugging I noticed that the first callback function is triggered the number of 'TasksToExecute', and then only the second callback function seems to be called. That is to say the 2 timers don't work simultaneously.
I have tried to increase p_maj_input and p_maj_output_motion to see if the problem is that matlab can't finish to run before another timer callback function is called but still for 5 subfolders I get the same problem.
Does anyone know where my problem is coming from?
This behavior occurs because one timer hasn't finished executing by the time it triggers again, so the second timer never has a chance to execute until the first timer is finished. If you change the ExecutionMode from 'fixedRate' to 'fixedSpacing', then you'll guarantee that there's time for the second timer to execute.
function duelingTimers()
disp('With ExecutionMode = fixedRate')
t1 = timer('ExecutionMode','fixedRate','Period',0.1,'TasksToExecute',5,'TimerFcn',#timerFcn1);
t2 = timer('ExecutionMode','fixedRate','Period',0.1,'TasksToExecute',5,'TimerFcn',#timerFcn2);
cleanup = onCleanup(#()delete([t1,t2]));
function timerFcn1(~,~)
pause(0.2)
disp('Timer 1');
end
function timerFcn2(~,~)
pause(0.2)
disp('Timer 2');
end
start(t1),start(t2)
wait(t1)
wait(t2)
disp(newline)
disp('With ExecutionMode = fixedSpacing')
t1.ExecutionMode = 'fixedSpacing';
t2.ExecutionMode = 'fixedSpacing';
start(t1),start(t2)
wait(t1)
wait(t2)
end
I am trying to read distance using Raspberry Pi Pico and ultrasonic distance sensor. While running the code in Thonny, I am getting the error,
TypeError: function missing 1 required positional arguments
The code is as below:
from machine import Pin, Timer
import utime
timer = Timer
trigger = Pin(3, Pin.OUT)
echo = Pin(2, Pin.IN)
distance = 0
def get_distance(timer):
global distance
trigger.high()
utime.sleep(0.00001)
trigger.low()
while echo.value() == 0:
start = utime.ticks_us()
while echo.value() == 1:
stop = utime.ticks_us()
timepassed = stop - start
distance = (timepassed * 0.0343) / 2
print("The distance from object is ",distance,"cm")
return distance
timer.init(freq=1, mode=Timer.PERIODIC, callback=get_distance)
while True:
get_distance()
utime.sleep(1)
Your initial problem is that you aren't using timer as an argument to your get_distance call, but you have bigger problems than that. You are using a timer to call get_distance but you are also calling get_distance in a loop. To top it off you have 2 blocking while loops in your get_distance function. Who knows how long the value of echo will stay 1 or 0. Will it stay one of those values longer than the next invocation from Timer? If so, you are going to have big problems. What you want to do is send periodic pulses to the pin to check the values. This can be done as below. This code isn't tested (although, it probably works). It is at least a solid gist of the direction you should be moving toward.
import machine, utime
trigger = machine.Pin(3, machine.Pin.OUT)
echo = machine.Pin(2, machine.Pin.IN)
def get_distance(timer):
global echo, trigger #you probably don't need this line
trigger.value(0)
utime.sleep_us(5)
trigger.value(1)
utime.sleep_us(10)
trigger.value(0)
pt = machine.time_pulse_us(echo, 1, 50000)
print("The distance from object is {} cm".format((pt/2)/29.1))
timer = machine.Timer(-1)
timer.init(mode=machine.Timer.PERIODIC, period=500, callback=get_distance)
Parts of this code were borrowed from here and reformatted to fit your design. I was too lazy to figure out how to effectively get rid of your while loops so, I just let the internet give me that answer (machine.time_pulse_us(echo, 1, 50000)).
Many of the ultrasonic units, such as the SRF04 nominally operate at 5V, so you could have problems if that's what you are using.
The v53l0x is a laser-based time-of flight device. It only works over short ranges (about a metre or so, but it definitely works with 3.3 V on a Pico with micropython and Thonny
https://www.youtube.com/watch?v=YBu6GKnN4lk
https://github.com/kevinmcaleer/vl53l0x
Hey guys so just as a disclaimer I'm relatively new to programming so if I'm making some super obvious mistake please go easy on me
So I'm trying to create a higher customizable Countdown timer for my game, and I want it to be able to be accurate to 0.01 Seconds. I decided I would use the Coroutine method for creating my timer instead of the delta-time one I have seen a couple of times, thinking that this would be a more efficient approach. My game is not very intensive and thus easily runs on hundreds of frames per second, so I thought that using Waitforseconds(0.01) is going to work better because it only needs to be called 100 times every second rather than multiple hundreds. however, I have come into a major issue with my timer. It is EXTREMELY slow. I ran the countdown timer on google and mine side by side starting at 25 seconds and it beat mine out by ten seconds. I even tried adding a artifical delay thinking the waitforseconds function was overshooting, so I would have the time tick down 0.01 seconds when a bit less then that had passed, but my results ended up being sort of inconsistent. Here is my code
using System.Collections;
using System.Collections.Generic;
using UnityEngine;
using UnityEngine.UI;
public class TimerScript : MonoBehaviour
{
public Text textDisplay;
private double secondsLeft = 30;
public bool takingAway = false;
private string Texttodisplay;
public int Milisecondsdigits = 2;
void Start()
{
textDisplay = GetComponent<Text>();
Texttodisplay = "00:" + secondsLeft;
if(Milisecondsdigits == 0)
{
Milisecondsdigits = -1;
}
}
void Update()
{
if (takingAway == false && secondsLeft > 0)
{
StopAllCoroutines();
StartCoroutine(TimerTake());
}
if(Texttodisplay.Length > 8 - (Mathf.Abs(Milisecondsdigits -2)))
{
Texttodisplay = Texttodisplay.Substring(0,8- (Mathf.Abs(Milisecondsdigits -2)));
}
textDisplay.text = Texttodisplay;
}
IEnumerator TimerTake()
{
takingAway = true;
yield return new WaitForSeconds(0.01f);
secondsLeft -= 0.01;
if(secondsLeft < 10)
{
Texttodisplay = "00:0" + secondsLeft;
}
else
{
Texttodisplay = "00:" + secondsLeft;
}
takingAway = false;
}
}
could somebody please let me know how I could cause this to become more accurate or why it's acting extremely inaccurate currently :/
Coroutine events like WaitForSeconds trigger at a defined point in Unity's event cycle, which takes place after Update() is processed (see
https://docs.unity3d.com/Manual/ExecutionOrder.html). This defined execution point might not line up exactly with the timer delay. This means it may wait longer than you want it to.
In your example, you tell it to wait for 0.01 seconds. Let's say that you are running a game at 30 frames per second. The frame time for 30 frames per second is 1/30 seconds, or approximately 0.0333 seconds. WaitForSeconds will then wait for the next frame, and 0.0333 seconds passes until the next frame. Then the next WaitForSeconds event cycle, it sees that the delay has passed and triggers, but you actually waited over 3 times as long as you wanted to because of the delay between event cycles. Since your code assumes that WaitForSeconds had only waited for 0.01 seconds, you will end up waiting longer than you intended to. This normally doesn't matter in a lot of applications, but when accumulating with frequent short delays it certainly does.
To solve this, you have two choices:
Accumulate time manually using Time.deltaTime in Update().
Coroutines likely check their completion status per frame in a
similar way in the yield WaitForSeconds event. If coroutines
check if they need to continue every frame, doing this manually with
Time.deltaTime might not be any less efficient at all than a
coroutine. You will have to benchmark to find out, because coroutines being more efficient isn't a safe assumption.
Use Time.time (or Time.realtimeSinceStartup if you want it unscaled) to measure the actual span of time that elapsed after the WaitForSeconds trigger, and use that as what you subtract from your remaining time.
There is also an additional consideration here that if you want your
text display to update at specific regular intervals, you will want
to dynamically adjust what value you pass into WaitForSeconds to
compensate for its drift.
Do you try to do it without Corroutine in a fixed update? Fixed update refresh every 0.02 seconds by default but you can settup to run in 0.01 seconds in Edit > Settings > Time > Fixed Timestep.
Replace the corroutine with a function in FixedUpdate
There is a link with better explation how works fixedupdate.
FixedUpdate Unity
I'm trying to write to a SSBO with a compute shader and read the data back on the cpu.
The compute shader is just a 1x1x1 toy example that writes 24 floats:
#version 450 core
layout(local_size_x = 1, local_size_y = 1, local_size_z = 1) in;
layout (std430, binding = 0) buffer particles {
float Particle[];
};
void main() {
for (int i = 0; i < 24; ++i) {
Particle[i] = i + 1;
}
}
This is how I run the shader and read the data:
val bufferFlags = GL_MAP_READ_BIT | GL_MAP_PERSISTENT_BIT | GL_MAP_COHERENT_BIT
val bufferSize = 24 * 4
val bufferId = glCreateBuffers()
glNamedBufferStorage(bufferId, bufferSize, bufferFlags)
val mappedBuffer = glMapNamedBufferRange(bufferId, 0, bufferSize, bufferFlags)
mappedBuffer.rewind()
val mappedFloatBuffer = mappedBuffer.asFloatBuffer()
mappedFloatBuffer.rewind()
val ssboIndex = glGetProgramResourceIndex(progId, GL_SHADER_STORAGE_BLOCK, "particles")
val props = Array(GL_BUFFER_BINDING)
val params = Array(-1)
glGetProgramResourceiv(progId, GL_SHADER_STORAGE_BLOCK, ssboIndex, props, null, params)
glBindBufferBase(GL_SHADER_STORAGE_BUFFER, params(0), bufferId)
glUseProgram(progId)
val sync = glFenceSync(GL_SYNC_GPU_COMMANDS_COMPLETE, 0)
glDispatchCompute(1, 1, 1)
glClientWaitSync(sync, 0, 1000000000) match {
case GL_TIMEOUT_EXPIRED =>
println("Timeout expired")
case GL_WAIT_FAILED =>
println("Wait failed. " + glGetError())
case _ =>
println("Result:")
while(mappedFloatBuffer.hasRemaining) {
println(mappedFloatBuffer.get())
}
}
I expect it to print the numbers 1 to 24 but instead it prints 24 zeros. Using the cpu I can read and write (if the GL_MAP_WRITE_BIT is set) to the buffer just fine. The same happens if I don't use DSA (glBindBuffer / glBufferStorage / glMapBufferRange instead). However, if the buffer isn't mapped while the shader runs and I only map it just before printing the contents, everything works correctly. Isn't this exactly what persistently mapped buffers are for? So I can keep it mapped while the gpu is using it?
I checked for any errors, with glGetError as well as with the newer debug output but I don't get any.
Here (pastebin) is a fully working example. You need LWJGL to run it.
There are a number of problems in your code.
First, you put the fence sync before the command you want to sync with. Syncing with a fence syncs with all commands executed before the fence, not after. If you want to sync with the compute shader execution, then you have to insert the fence after the dispatch call, not before.
Second, synchronization is not enough. Writes to an SSBO are incoherent, so you must follow the rules of incoherent memory accesses in order to make them visible to you. In this case, you need to insert an appropriate memory barrier between the compute operation and when you try to read from the buffer with glMemoryBarrier. Since you're reading the data via mapping, the proper barrier to use is GL_CLIENT_MAPPED_BUFFER_BARRIER_BIT.
Your code appears to work when you use non-persistent mapping, but this is mere appearance. It's still undefined behavior due to the improper incoherent memory access (ie: the lack of a memory barrier). It just so happens that the UB does what you want... in that case.
Is there a way to have a MATLAB timer pass different data on each subsequent call to the timer function? My goal is to cycle through intervals at a fixed rate, and the pause function inside a loop is not precise enough.
I have workng MATLAB code that uses a for loop to send data via serial ports, then wait a specified time before the next iteration of the loop. The serial communication varies in speed, so if I specify 300 seconds as the period, the loop actually executes every 340-360 seconds. Here is the existing code:
clear all;
testFile = input('What is the name of the test data file (with extension): ', 's');
measurementData = csvread(testFile);
intervalDuration = input('What is the measurement change period (seconds): ');
intervalNumber = size(measurementData,2);
% Set up the COM PORT communication
sensorComPort = [101,102,103,104,105,106,107,108,109,110,111,112,113,114,115,116,117,118,119,120];
controllerComPort = [121,122,123,124];
for j=1:intervalNumber
tic
fprintf('\nInterval # %2d\n',rem(j,24));
sensorMeasurementPS = [measurementData(1,j),measurementData(2,j),measurementData(3,j),measurementData(4,j),measurementData(5,j), measurementData(6,j),measurementData(7,j),measurementData(8,j),measurementData(9,j),measurementData(10,j), measurementData(11,j),measurementData(12,j),measurementData(13,j),measurementData(14,j),measurementData(15,j), measurementData(16,j),measurementData(17,j),measurementData(18,j),measurementData(19,j),measurementData(20,j)];
serialSensorObj = startsensorPSWithoutReset(sensorComPort, sensorMeasurementPS);
serialSensorObj = changeMeasurement(serialSensorObj, sensorMeasurementPS);
rc = stopsensorPS(serialSensorObj);
controllerMeasurementPS = [measurementData(21,j),measurementData(22,j),measurementData(23,j),measurementData(24,j)];
serialControllerObj = startControllerPSWithoutReset(controllerComPort, controllerMeasurementPS);
serialControllerObj = changeMeasurement(serialControllerObj, controllerMeasurementPS);
rc2 = stopControllerPS(serialControllerObj);
pause(intervalDuration);
t = toc;
fprintf('Elapsed time = %3.4f\n',t);
end
clear serialSensorObj;
clear serialControllerObj;
The serial functions are specified in other files and they are working as intended.
What I need to do is have the serial communication execute on a more precise 5-minute interval. (The actual timing of the commands inside the interval will still vary slightly, but the commands will kick off every 5 minutes over the course of 24 hours. The current version loses time and gets out of sync with another system that is reading the measurements I'm setting by serial port.)
My first thought is to use a MATLAB timer with the fixedRate execution mode, which queues the function at fixed intervals. However, it doesn't appear that I can send the timer function different data for each interval. I thought about having the timer function change a counter in the workspace, similar to j in my existing for loop, but I know that having functions interact with the workspace is not recommended.
Here's what I've come up with so far for the timer method:
function [nextJ] = changeMeasurement_fcn(obj,event,j,sensorComPort,controllerComPort)
tic
fprintf('\nInterval # %2d\n',rem(j,24));
sensorMeasurementPS = [measurementData(1,j),measurementData(2,j),measurementData(3,j),measurementData(4,j),measurementData(5,j), measurementData(6,j),measurementData(7,j),measurementData(8,j),measurementData(9,j),measurementData(10,j), measurementData(11,j),measurementData(12,j),measurementData(13,j),measurementData(14,j),measurementData(15,j), measurementData(16,j),measurementData(17,j),measurementData(18,j),measurementData(19,j),measurementData(20,j)];
serialSensorObj = startSensorPSWithoutReset(sensorComPort, sensorMeasurementPS);
serialSensorObj = changeMeasurement(serialSensorObj, sensorMeasurementPS);
rc = stopSensorPS(serialSensorObj);
controllerMeasurementPS = [measurementData(21,j),measurementData(22,j),measurementData(23,j),measurementData(24,j)];
serialControllerObj = startControllerPSWithoutReset(controllerComPort, controllerMeasurementPS);
serialControllerObj = changeMeasurement(serialControllerObj, controllerMeasurementPS);
rc2 = stopControllerPS(serialControllerObj);
t2 = toc;
fprintf('Elapsed time = %3.4f\n',t2);
and this is how I would call it from the main m file:
t = timer('TimerFcn',#changeMeasurement,'ExecutionMode','fixedRate','period',intervalDuration);
% then I need some code to accept the returned nextJ from the timer function
This feels a bit sloppy so I'm hoping there's a built-in way to have a timer cycle through a data set.
Another idea I had was to keep the for loop but change the pause function to use a value calculated based on how much time would add up to 5 minutes for the iteration.
To summarize my question:
a) Can I have a timer pass different data to the timer function on each iteration?
b) Is that a good way to go about cycling through the intervals in my data on a precise 5-minute interval?
Thanks!
I stumbled on this page: http://www.mathworks.com/company/newsletters/articles/tips-and-tricks-simplifying-measurement-and-timer-callbacks-with-nested-functions-new-online-support-features.html
and learned that timer callback functions can be nested inside other functions (but not regular scripts).
Using that information, I cut my scenario to the basics and came up with this code:
function timerTestMain_fcn
testFile = input('What is the name of the test data file (with extension): ', 's');
testData = csvread(testFile);
intervalDuration = input('What is the voltage change period (seconds): ');
intervalNumber = size(testData,2);
t = timer('ExecutionMode','fixedRate','period',intervalDuration,'TasksToExecute',intervalNumber);
t.TimerFcn = {#timerTest_fcn};
start(t);
wait(t);
delete(t);
function timerTest_fcn(obj,event)
tic
event_time = datestr(event.Data.time);
interval_id = t.TasksExecuted;
data_value = testData(1,interval_id);
txt1 = 'Interval ';
txt2 = num2str(interval_id);
txt3 = ' occurred at ';
txt4 = ' with data value of ';
txt5 = num2str(data_value);
msg = [txt1 txt2 txt3 event_time txt4 txt5];
disp(msg)
t2 = toc;
fprintf('Elapsed time = %3.4f\n',t2);
end
end
The test data file it requests must be a csv containing a row vector. For example, you could put the values 11,12,13,14,15 across the first row of the csv. The output message would then say 'Interval 1 occurred at [time] with data value of 11', 'Interval 2 occurred at [time] with data value of 12', etc.
The key is that by nesting the functions, the timer callback can reference both the test data and the timer attributes contained in the outer function. The TasksExecuted property of the timer serves as the counter.
Thanks to anyone who thought about my question. Comments welcome.