child class of leafsystem generating sinusoidal signal - drake

I am trying to make a child class of LeafSystem whose output is sinusoidal and its derivative.
I wrote the code and try to plot it but signal logger doesn't log correctly.
#include "drake/systems/framework/leaf_system.h"
#include "drake/systems/analysis/simulator.h"
#include "drake/systems/framework/diagram.h"
#include "drake/systems/framework/diagram_builder.h"
#include "drake/systems/primitives/signal_logger.h"
#include "drake/common/proto/call_python.h"
class Sinusoid : public drake::systems::LeafSystem<double>
{
public:
Sinusoid (double tstart, double freq, double amp, double offset) :
m_freq(freq), m_amp(amp), m_offset(offset), m_tstart(tstart) {
this->DeclareVectorOutputPort(
drake::systems::BasicVector<double>(2), &Sinusoid::output);
}
private:
void output(const drake::systems::Context<double>& c, drake::systems::BasicVector<double>* output) const {
double t(c.get_time());
double tknot(t - m_tstart);
if (t > m_tstart) {
output->SetAtIndex(0, std::sin(tknot*m_freq + m_offset)*m_amp);
output->SetAtIndex(1, std::cos(tknot*m_freq + m_offset)*m_amp*m_freq);
} else {
output->SetAtIndex(0, 0.0);
output->SetAtIndex(1, 0.0);
}
}
double m_freq{0.0}, m_amp{0.0}, m_offset{0.0}, m_tstart{0.0};
};
int main(int argc, char *argv[])
{
// Add System and Connect
drake::systems::DiagramBuilder<double> builder;
auto system = builder.AddSystem<Sinusoid>(1.0, 2.*M_PI*1., 3., 0.);
auto logger = LogOutput(system->get_output_port(0), &builder);
auto diagram = builder.Build();
// Construct Simulator
drake::systems::Simulator<double> simulator(*diagram);
// Run simulation
simulator.StepTo(100);
// Plot with Python
auto sample_time = logger->sample_times();
auto sample_data = logger->data();
std::cout << sample_time.size() << std::endl;
for (int i = 0; i < sample_time.size(); ++i) {
std::cout << sample_time(i) << " : " << sample_data(i, 0) << " " << sample_data(i, 1) << std::endl;
}
std::cout << "END" << std::endl;
return 0;
}
The output of the code is
2
0 : 0 0
0 : 0 0
END
Whatever number I used in StepTo function, signal logger only cate 2 data whose sampled times are both 0.

The code looks good. Note that TrajectorySource does this almost exactly (and used SingleOutputVectorSource as a base class, which you might consider, too). The only problem is that you do not have anything telling the simulator that there is a reason to evaluate the output port. The logger block will pull on that for every publish event, but you haven't told the simulator to publish.
The solution is to call
simulator.set_publish_every_timestep(true)
http://drake.mit.edu/doxygen_cxx/classdrake_1_1systems_1_1_simulator.html#aef1dc6aeb821503379ab1dd8c6044562
If you want to further control the timestep of the integrator, you could set the parameters of the integrator (e.g. simalator.get_integerator), then calls like set_fixed_step_mode.

Related

Why the interpreter complains that library named "math" does not exist?

Why the interpreter complains that library named "math" does not exist?
As far as I know, this library is loaded when invoking luaL_newstate on Lua-5.3.5.
#include "lua.hpp"
#include <iostream>
#include <assert.h>
#include <fstream>
int main()
{
struct lua_State *L = luaL_newstate();
int ret;
std::string fileName("co.lua");
if(fileName.empty())
{
std::cout << "the filename is empty" << std::endl;
return -1;
}
std::ifstream fileScript(fileName, fileScript.in|std::ios::ate);
if(!fileScript.is_open())
{
std::cout << "open file failed" << std::endl;
return -2;
}
size_t size = fileScript.tellg();
if(size <= 0)
{
std::cout << "file has no valid content" << std::endl;
return -3;
}
std::string textCont(size, '\0');
fileScript.seekg(0);
fileScript.read(&textCont[0], size);
if((ret=luaL_loadbuffer(L, textCont.data(), textCont.length(), "co.lua")) == LUA_OK)
{
if((ret=lua_pcall(L, 0, LUA_MULTRET, 0)) != LUA_OK)
{
std::cout << "error in invoking lua_pcall():" << ret << std::endl;
if(lua_isstring(L, -1))
{
const char *errMsg = lua_tostring(L, -1);
lua_pop(L, 1);
std::cout << "script run encounter err:" << errMsg << std::endl;
}
}
}
}
Here is the code snippet(it's very simple) for the file named "co.lua":
a = 1;
b=2;
a=a+1;
math.sin(a)
Here is the error message in the console:
error in invoking lua_pcall():2
script run encounter err:[string "co.lua"]:29: attempt to index a nil value (global 'math')
The documentation states that you need to call luaL_openlibs or luaL_requiref which does not seem to be the case with your posted program.
To have access to these libraries, the C host program should call the luaL_openlibs function, which opens all standard libraries.
Alternatively (emphasis mine):
Alternatively, the host program can open them individually by using luaL_requiref to call:
luaopen_base (for the basic library)
luaopen_package (for the package library)
luaopen_coroutine (for the coroutine library)
luaopen_string (for the string library)
luaopen_utf8 (for the UTF8 library)
luaopen_table (for the table library)
luaopen_math (for the mathematical library)
luaopen_io (for the I/O library)
luaopen_os (for the operating system library)
luaopen_debug (for the debug library).
These functions are declared in lualib.h.
So change your program's first few lines to something like below.
You also need to compare the return value from luaL_newstate with NULL and handle that error condition.
int main()
{
struct lua_State *L = luaL_newstate();
if( L == NULL ) {
puts( "Lua failed to initialize." );
exit(1);
}
luaL_openlibs( L );
// etc

Searching Text in Memory without Visual Component in C++ Builder

I am using C++Builder 10.3 with the VCL 32bit platform. I need to know the best way to search a text file in memory. I wrote the code below which opens a text file into the RichEdit component and searches for and selects some text. The RichEdit is intended to be used as a Visual Component. The TMemoryStream and TStringStream are used in memory but do not offer the methods FindText, SelStart, SelLength and SelText. Can you show how to do this in memory?
UnicodeString MyCrumb;
int StartPos=0, ToEnd=0, FoundAt=0, StartCrumb=0;
TSearchTypes mySearchTypes = TSearchTypes();
RichEdit1->Lines->LoadFromFile( "CrumbFile.txt" );
ToEnd = RichEdit1->Text.Length();
FoundAt = RichEdit1->FindText(L"CrumbStore", StartPos, ToEnd, mySearchTypes);
StartPos = FoundAt+10;
FoundAt = RichEdit1->FindText("crumb", StartPos, ToEnd, mySearchTypes);
StartPos = FoundAt+8;
StartCrumb = FoundAt+8;
FoundAt = RichEdit1->FindText("}", StartPos, ToEnd, mySearchTypes);
EndPos = FoundAt-1;
RichEdit1->SelStart = StartPos;
RichEdit1->SelLength = ( EndPos-StartPos );
MyCrumb = RichEdit1->SelText;
The VCL way is to use TStringList class instead of visual components. However, entire file will be loaded in the memory.
#include <iostream>
#include <memory>
using namespace std;
void FindTextVcl()
{
unique_ptr<TStringList> txt(new TStringList());
txt->LoadFromFile(L"Example.txt"); // Use appropriate TEncoding if need
for (int line_num = 0; line_num != txt->Count; line_num++)
{
int pos = txt->Strings[line_num].Pos("there");
if (pos > 0)
{
cout << "Found at line " << line_num + 1 << ", position " << pos << endl;
break;
}
}
}
The standard library way is like the following example (use wstring and wifstream for UTF-16).
This works for big files because only current string is loaded in the memory.
#include <iostream>
#include <fstream>
#include <string>
using namespace std;
void FindTextStd()
{
ifstream txt("Example.txt");
if (txt.is_open())
{
size_t pos = 0;
size_t line_num = 0;
string line;
while (getline(txt, line))
{
line_num++;
pos = line.find("there");
if (pos != string::npos)
{
cout << "Found at line " << line_num << ", position " << pos + 1 << endl;
break;
}
}
}
}

Is this the correct way to thread sync with out mutex

Is this the correct way to sync threads without mutex.
This code should be running for a long time
#include <boost/thread.hpp>
#include <boost/thread/mutex.hpp>
#include <boost/memory_order.hpp>
#include <atomic>
std::atomic<long> x =0;
std::atomic<long> y =0;
boost::mutex m1;
// Thread increments
void Thread_Func()
{
for(;;)
{
// boost::mutex::scoped_lock lx(m1);
++x;
++y;
}
}
// Checker Thread
void Thread_Func_X()
{
for(;;)
{
// boost::mutex::scoped_lock lx(m1);
if(y > x)
{
// should never hit until int overflows
std::cout << y << "\\" << x << std::endl;
break;
}
}
}
//Test Application
int main(int argc, char* argv[])
{
boost::thread_group threads;
threads.create_thread(Thread_Func);
threads.create_thread(Thread_Func_X);
threads.join_all();
return 0;
}
Without knowing exactly what you're trying to do, it is hard to say it is the "correct" way. That's valid code, it's a bit janky though.
There is no guarantee that the "Checker" thread will ever see the condition y > x. It's theoretically possible that it will never break. In practice, it will trigger at some point but x might not be LONG_MIN and y LONG_MAX. In other words, it's not guaranteed to trigger just as the overflow happens.

After reading multiple frames from a camera, OpenCV suddenly always fails to read frames. How do I diagnose this?

I run a program similar to the one in this question: https://stackoverflow.com/a/8719192/26070
#include <opencv/highgui.h>
#include <iostream>
/** #function main */
int main( int argc, char** argv )
{
cv::VideoCapture vcap;
cv::Mat image;
const std::string videoStreamAddress = "rtsp://192.0.0.1:8081/live.sdp";
//open the video stream and make sure it's opened
if(!vcap.open(videoStreamAddress)) {
std::cout << "Error opening video stream or file" << std::endl;
return -1;
}
for(;;) {
if(!vcap.read(image)) {
std::cout << "No frame" << std::endl;
cv::waitKey(500);
} else {
cv::imshow("Output Window", image);
}
if(cv::waitKey(1) >= 0) break;
}
}
The program runs fine for a certain amount of time (about one minute or so) and then the call to read() (method from cv::VideoCapture) always returns false.
The output is as follows:
[mpeg4 # 00da27a0] ac-tex damaged at 22 7
[mpeg4 # 00da27a0] Error at MB: 309
No frame
No frame
No frame
Note: the first two lines are not always present.
So, how can I determine what the root of the problem is?

Forcing a Lua script to exit

How do you end a long running Lua script?
I have two threads, one runs the main program and the other controls a user supplied Lua script. I need to kill the thread that's running Lua, but first I need the script to exit.
Is there a way to force a script to exit?
I have read that the suggested approach is to return a Lua exception. However, it's not garanteed that the user's script will ever call an api function ( it could be in a tight busy loop). Further, the user could prevent errors from causing his script to exit by using a pcall.
You could use setjmp and longjump, just like the Lua library does internally. That will get you out of pcalls and stuff just fine without need to continuously error, preventing the script from attempting to handle your bogus errors and still getting you out of execution. (I have no idea how well this plays with threads though.)
#include <stdio.h>
#include <setjmp.h>
#include "lua.h"
#include "lualib.h"
#include "lauxlib.h"
jmp_buf place;
void hook(lua_State* L, lua_Debug *ar)
{
static int countdown = 10;
if (countdown > 0)
{
--countdown;
printf("countdown: %d!\n", countdown);
}
else
{
longjmp(place, 1);
}
}
int main(int argc, const char *argv[])
{
lua_State* L = luaL_newstate();
luaL_openlibs(L);
lua_sethook(L, hook, LUA_MASKCOUNT, 100);
if (setjmp(place) == 0)
luaL_dostring(L, "function test() pcall(test) print 'recursing' end pcall(test)");
lua_close(L);
printf("Done!");
return 0;
}
You could set a variable somewhere in your program and call it something like forceQuitLuaScript. Then, you use a hook, described here to run every n instructions. After n instructions, it'll run your hook which just checks if forceQuitLuaScript is set, and if it is do any clean up you need to do and kill the thread.
Edit: Here's a cheap example of how it could work, only this is single threaded. This is just to illustrate how you might handle pcall and such:
#include <stdlib.h>
#include "lauxlib.h"
void hook(lua_State* L, lua_Debug *ar)
{
static int countdown = 10;
if (countdown > 0)
{
--countdown;
printf("countdown: %d!\n", countdown);
}
else
{
// From now on, as soon as a line is executed, error
// keep erroring until you're script reaches the top
lua_sethook(L, hook, LUA_MASKLINE, 0);
luaL_error(L, "");
}
}
int main(int argc, const char *argv[])
{
lua_State* L = luaL_newstate();
luaL_openlibs(L);
lua_sethook(L, hook, LUA_MASKCOUNT, 100);
// Infinitely recurse into pcalls
luaL_dostring(L, "function test() pcall(test) print 'recursing' end pcall(test)");
lua_close(L);
printf("Done!");
return 0;
}
The way to end a script is to raise an error by calling error. However, if the user has called the script via pcall then this error will be caught.
It seems like you could terminate the thread externally (from your main thread) since the lua script is user supplied and you can't signal it to exit.
If that isn't an option, you could try the debug API. You could use lua_sethook to enable you to regain control assuming you have a way to gracefully terminate your thread in the hook.
I haven't found a way to cleanly kill a thread that is executing a long running lua script without relying on some intervention from the script itself. Here are some approaches I have taken in the past:
If the script is long running it is most likely in some loop. The script can check the value of some global variable on each iteration. By setting this variable from outside of the script you can then terminate the thread.
You can start the thread by using lua_resume. The script can then exit by using yield().
You could provide your own implementation of pcall that checks for a specific type of error. The script could then call error() with a custom error type that your version of pcall could watch for:
function()
local there_is_an_error = do_something()
if (there_is_an_error) then
error({code = 900, msg = "Custom error"})
end
end
possibly useless, but in the lua I use (luaplayer or PGELua), I exit with
os.exit()
or
pge.exit()
If you're using coroutines to start the threads, you could maybe use coroutine.yield() to stop it.
You might wanna take look at
https://github.com/amilamad/preemptive-task-scheduler-for-lua
project. its preemptive scheduler for lua.
It uses a lua_yeild function inside the hook. So you can suspend your lua thread. It also uses longjmp inside but its is much safer.
session:destroy();
Use this single line code on that where you are want to destroy lua script.
lua_KFunction cont(lua_State* L);
int my_yield_with_res(lua_State* L, int res) {
cout << " my_yield_with_res \n" << endl;
return lua_yieldk(L, 0, lua_yield(L, res), cont(L));/* int lua_yieldk(lua_State * L, int res, lua_KContext ctx, lua_KFunction k);
Приостанавливает выполнение сопрограммы(поток). Когда функция C вызывает lua_yieldk, работающая
сопрограмма приостанавливает свое выполнение и вызывает lua_resume, которая начинает возврат данной сопрограммы.
Параметр res - это число значений из стека, которые будут переданы в качестве результатов в lua_resume.
Когда сопрограмма снова возобновит выполнение, Lua вызовет заданную функцию продолжения k для продолжения выполнения
приостановленной C функции(смотрите §4.7). */
};
int hookFunc(lua_State* L, lua_Debug* ar) {
cout << " hookFunc \n" << endl;
return my_yield_with_res(L, 0);// хук./
};
lua_KFunction cont(lua_State* L) {// функция продолжения.
cout << " hooh off \n" << endl;
lua_sethook(L, (lua_Hook)hookFunc, LUA_MASKCOUNT, 0);// отключить хук foo.
return 0;
};
struct Func_resume {
Func_resume(lua_State* L, const char* funcrun, unsigned int Args) : m_L(L), m_funcrun(funcrun), m_Args(Args) {}
//имена функций, кол-во агрументов.
private:
void func_block(lua_State* L, const char* functionName, unsigned int Count, unsigned int m_Args) {
lua_sethook(m_L, (lua_Hook)hookFunc, LUA_MASKCOUNT, Count); //вызов функции с заданной паузой.
if (m_Args == 0) {
lua_getglobal(L, functionName);// получить имя функции.
lua_resume(L, L, m_Args);
}
if (m_Args != 0) {
int size = m_Args + 1;
lua_getglobal(L, functionName);
for (int i = 1; i < size; i++) {
lua_pushvalue(L, i);
}
lua_resume(L, L, m_Args);
}
};
public:
void Update(float dt) {
unsigned int Count = dt * 100.0;// Время работы потока.
func_block(m_L, m_funcrun, Count, m_Args);
};
~Func_resume() {}
private:
lua_State* m_L;
const char* m_funcrun; // имя функции.
unsigned int m_Count;// число итерации.
unsigned int m_Args;
};
const char* LUA = R"(
function main(y)
--print(" func main arg, a = ".. a.." y = ".. y)
for i = 1, y do
print(" func main count = ".. i)
end
end
)";
int main(int argc, char* argv[]) {
lua_State* L = luaL_newstate();/*Функция создает новое Lua состояние. */
luaL_openlibs(L);
luaL_dostring(L, LUA);
//..pushlua(L, 12);
pushlua(L, 32);
//do {
Func_resume func_resume(L, "main", 2);
func_resume.Update(1.7);
lua_close(L);
// } while (LUA_OK != lua_status(L)); // Пока поток не завершен.
return 0;
};

Resources