void Update() override final {
if (lastActivated >= 0) {
InputPort<AnyType>* port = static_cast<InputPort<AnyType>*>(GetInput(lastActivated));
GetOutput<0>().Set(*port->Get());
lastActivated = -1;
}
}
void updateHook() {
dummyInPort.read(dataVar);
dummyOutPort.write(dataVar);
dataInPort.read(dataVar);
dataOutPort.write(dataVar);
//log(Info) << "Worker relaying data!" << endlog();
}
void Connection::mouseReleaseEvent(QGraphicsSceneMouseEvent *event)
{
QGraphicsObject::mouseReleaseEvent(event);
if (drag_state == CONNECTED)
return;
ungrabMouse();
clearFocus();
setFlag(QGraphicsItem::ItemIsFocusable, false);
InputPort* target = gscene()->getInputPortAt(endPos());
Datum* datum = target ? target->getDatum() : NULL;
if (target && datum->acceptsLink(link))
{
datum->addLink(link);
drag_state = CONNECTED;
connect(endInspector(), &NodeInspector::moved,
this, &Connection::onInspectorMoved);
connect(endInspector(), &NodeInspector::hiddenChanged,
this, &Connection::onHiddenChanged);
App::instance()->pushStack(new UndoAddLinkCommand(link));
}
else
{
link->deleteLater();
}
prepareGeometryChange();
}
void Connection::mouseReleaseEvent(QGraphicsSceneMouseEvent *event)
{
QGraphicsObject::mouseReleaseEvent(event);
if (drag_state == CONNECTED)
return;
ungrabMouse();
clearFocus();
setFlag(QGraphicsItem::ItemIsFocusable, false);
InputPort* t = gscene()->getInputPortAt(endPos());
Datum* datum = target ? target->getDatum() : NULL;
if (t && datum->acceptsLink(source->getDatum()))
{
target = t;
t->install(this);
datum->installLink(source->getDatum());
drag_state = CONNECTED;
App::instance()->pushStack(
new UndoAddLinkCommand(source->getDatum(), datum));
onPortsMoved();
emit(changed());
}
else
{
deleteLater();
}
prepareGeometryChange();
}
void
ConnectionInterface::PushConsumer ( InputPort& consumer )
{
if ( _consumers.find ( consumer.GetID() ) == _consumers.end() ) {
consumer.Plug ( *this );
_consumers[ consumer.GetID() ] = &consumer;
} else {
//TODO _THROW_ exception
}
}
void CorbaMQueueIPCTest::testPortBufferConnection()
{
// This test assumes that there is a buffer connection mw1 => server => mr1 of size 3
// Check if connection succeeded both ways:
BOOST_CHECK( mw1->connected() );
BOOST_CHECK( mr1->connected() );
double value = 0;
// Check if no-data works
BOOST_CHECK( !mr1->read(value) );
// Check if writing works
ASSERT_PORT_SIGNALLING(mw1->write(1.0), mr1);
ASSERT_PORT_SIGNALLING(mw1->write(2.0), mr1);
ASSERT_PORT_SIGNALLING(mw1->write(3.0), mr1);
// There are two connections between mw1 and mr1. We first flush one of the
// two, and then the second one (normal multi-writer single-reader
// behaviour)
BOOST_CHECK( mr1->read(value) );
BOOST_CHECK_EQUAL( 1.0, value );
BOOST_CHECK( mr1->read(value) );
BOOST_CHECK_EQUAL( 2.0, value );
BOOST_CHECK( mr1->read(value) );
BOOST_CHECK_EQUAL( 3.0, value );
BOOST_CHECK( mr1->read(value) );
BOOST_CHECK_EQUAL( 1.0, value );
BOOST_CHECK( mr1->read(value) );
BOOST_CHECK_EQUAL( 2.0, value );
BOOST_CHECK( mr1->read(value) );
BOOST_CHECK_EQUAL( 3.0, value );
BOOST_CHECK_EQUAL( mr1->read(value), OldData );
}
void
ConnectionInterface::DisconnectConsumer ( InputPort& inputPort )
{
ConsumersMap::iterator it = _consumers.find ( inputPort.GetID() );
if ( it != _consumers.end() ) {
inputPort.UnPlug ( true );
_consumers.erase ( it );
} else {
//TODO _THROW_ exception
}
}
InputPort* NodeInspector::inputPort(const Datum* d) const
{
for (auto row : rows)
for (auto a : row->childItems())
{
InputPort* p = dynamic_cast<InputPort*>(a);
if (p && p->getDatum() == d)
return p;
}
return NULL;
}
virtual void disconnect(bool forward)
{
InputPort<T>* port = this->port;
this->port = 0;
if (forward)
{
if (port)
port->removeConnection(cid);
}
else
base::ChannelElement<T>::disconnect(false);
}
wxString
MethodSameAs::GetType(const OutputPort *output) const
{
Node *node = static_cast<Node *>(output->GetParent());
for (List<InputPort *>::Iterator i = node->InputIterator(); !i; i++)
{
InputPort *input = *i;
if (input->GetLuaId() == m_id)
{
return input->GetType();
}
}
return wxT("?");
}
void updateHook() {
double input_sample;
if ( input.read(input_sample) ) {
current += this->engine()->getActivity()->getPeriod()*input_sample /inertia.value();
}
output.write( current );
}
void updateHook()
{
if(NewData==posInc_in.read(posIncData) && NewData==deltaInc_in.read(deltaIncData))
{
/*for(int i=0; i<number_of_drives; i++)
{
std::cout << "POS INC" << posIncData[i] << std::endl;
}*/
computedPwm_out.write(computePwmValue(posIncData,deltaIncData));
}
/*else
std::cout << "NO DATA" << std::endl;
*/
}
static bool createStream(InputPort<T>& input_port, ConnPolicy const& policy)
{
StreamConnID *sid = new StreamConnID(policy.name_id);
RTT::base::ChannelElementBase::shared_ptr outhalf = buildChannelOutput( input_port, sid );
if ( createAndCheckStream(input_port, policy, outhalf, sid) )
return true;
input_port.removeConnection(sid);
return false;
}
void CorbaTest::testPortDataConnection()
{
// This test assumes that there is a data connection mo1 => mi2
// Check if connection succeeded both ways:
BOOST_CHECK( mo1->connected() );
BOOST_CHECK( mi2->connected() );
double value = 0;
// Check if no-data works
BOOST_CHECK_EQUAL( mi2->read(value), NoData );
// Check if writing works (including signalling)
ASSERT_PORT_SIGNALLING(mo1->write(1.0), mi2)
BOOST_CHECK( mi2->read(value) );
BOOST_CHECK_EQUAL( 1.0, value );
ASSERT_PORT_SIGNALLING(mo1->write(2.0), mi2);
BOOST_CHECK( mi2->read(value) );
BOOST_CHECK_EQUAL( 2.0, value );
}
void CorbaMQueueIPCTest::testPortDataConnection()
{
// This test assumes that there is a data connection mw1 => server => mr1
// Check if connection succeeded both ways:
BOOST_CHECK( mw1->connected() );
BOOST_CHECK( mr1->connected() );
double value = 0;
// Check if no-data works
BOOST_CHECK( !mr1->read(value) );
// Check if writing works (including signalling)
ASSERT_PORT_SIGNALLING(mw1->write(1.0), mr1)
BOOST_CHECK( mr1->read(value) );
BOOST_CHECK_EQUAL( 1.0, value );
ASSERT_PORT_SIGNALLING(mw1->write(2.0), mr1);
BOOST_CHECK( mr1->read(value) );
BOOST_CHECK_EQUAL( 2.0, value );
}
int
InputPort::LuaGetSource(lua_State *L)
{
ShaderObject *obj = *(ShaderObject **)lua_touserdata(L, lua_upvalueindex(1));
InputPort *input = dynamic_cast<InputPort *>(obj);
OutputPort *source = input->GetSource();
if (source != NULL)
{
source->PushLua(L);
}
else
{
lua_newtable(L);
lua_pushcfunction(L, ::LuaUID);
lua_setfield(L, -2, "uid");
lua_pushcfunction(L, ::LuaGetType);
lua_setfield(L, -2, "get_type");
}
return 1;
}
void CorbaTest::testPortBufferConnection()
{
// This test assumes that there is a buffer connection mo => mi of size 3
// Check if connection succeeded both ways:
BOOST_CHECK( mo->connected() );
BOOST_CHECK( mi->connected() );
double value = 0;
// Check if no-data works
BOOST_CHECK_EQUAL( mi->read(value), NoData );
// Check if writing works
ASSERT_PORT_SIGNALLING(mo->write(1.0), mi);
ASSERT_PORT_SIGNALLING(mo->write(2.0), mi);
ASSERT_PORT_SIGNALLING(mo->write(3.0), mi);
ASSERT_PORT_SIGNALLING(mo->write(4.0), 0);
BOOST_CHECK( mi->read(value) );
BOOST_CHECK_EQUAL( 1.0, value );
BOOST_CHECK( mi->read(value) );
BOOST_CHECK_EQUAL( 2.0, value );
BOOST_CHECK( mi->read(value) );
BOOST_CHECK_EQUAL( 3.0, value );
BOOST_CHECK_EQUAL( mi->read(value), OldData );
}
void CorbaMQueueIPCTest::testPortBufferConnection()
{
// This test assumes that there is a buffer connection mw1 => server => mr1 of size 3
// Check if connection succeeded both ways:
BOOST_CHECK( mw1->connected() );
BOOST_CHECK( mr1->connected() );
double value = 0;
// Check if no-data works
BOOST_CHECK( !mr1->read(value) );
// Check if writing works
ASSERT_PORT_SIGNALLING(mw1->write(1.0), mr1);
ASSERT_PORT_SIGNALLING(mw1->write(2.0), mr1);
ASSERT_PORT_SIGNALLING(mw1->write(3.0), mr1);
// it will be emptied too fast by mqueue.
//ASSERT_PORT_SIGNALLING(mw1->write(4.0), 0);
BOOST_CHECK( mr1->read(value) );
BOOST_CHECK_EQUAL( 1.0, value );
BOOST_CHECK( mr1->read(value) );
BOOST_CHECK_EQUAL( 2.0, value );
BOOST_CHECK( mr1->read(value) );
BOOST_CHECK_EQUAL( 3.0, value );
BOOST_CHECK_EQUAL( mr1->read(value), OldData );
}
InputPort* Canvas::getInputPortNear(QPointF pos, Link* link) const
{
float distance = INFINITY;
InputPort* port = NULL;
for (auto i : scene->items())
{
InputPort* p = dynamic_cast<InputPort*>(i);
if (p && (link == NULL || p->getDatum()->acceptsLink(link)))
{
QPointF delta = p->mapToScene(p->boundingRect().center()) - pos;
float d = QPointF::dotProduct(delta, delta);
if (d < distance)
{
distance = d;
port = p;
}
}
}
return port;
}
void Update() override final {
if (lastActivated >= 0 && activationMap == 0b11'1111) {
InputPort<AnyType>* port = static_cast<InputPort<AnyType>*>(GetInput(lastActivated+1));
GetOutput<0>().Set(*port->Get());
}
}
InputPortSource(InputPort<T>& port)
: port(&port), mvalue() {
port.getDataSample( mvalue );
}
bool evaluate() const
{
return port->read(mvalue, false) == NewData;
}
void reset() { port->clear(); }
void MaterialShaderGraph::AssembleShaderCode() {
std::vector<ShaderNode> shaderNodes(m_nodes.size());
std::vector<std::vector<MaterialShaderParameter>> shaderNodeParams(m_nodes.size());
std::vector<eMaterialShaderParamType> shaderNodeReturns(m_nodes.size());
std::vector<std::string> functions(m_nodes.size());
// collect individual shader codes and set number of input params for each node
for (size_t i = 0; i < m_nodes.size(); ++i) {
MaterialShader* shader = m_nodes[i].get();
functions[i] = shader->GetShaderCode();
ExtractShaderParameters(functions[i], "main", shaderNodeReturns[i], shaderNodeParams[i]);
for (auto p : shaderNodeParams[i]) {
if (p.type == eMaterialShaderParamType::UNKNOWN) {
throw InvalidArgumentException("Parameter of unknown type.");
}
}
shaderNodes[i].SetNumInputs(shaderNodeParams[i].size());
}
// link nodes together
for (const auto& link : m_links) {
ShaderNode& source = shaderNodes[link.sourceNode];
ShaderNode& sink = shaderNodes[link.sinkNode];
if (sink.GetNumInputs() <= link.sinkPort) {
throw InvalidArgumentException("Invalid link: port does not have that many inputs.");
}
bool isLinked = source.GetOutput(0)->Link(sink.GetInput(link.sinkPort));
if (!isLinked) {
throw InvalidArgumentException("Invalid link: duplicate input to a single port.");
}
}
// create output port LUT
std::unordered_map<OutputPortBase*, size_t> outputLut;
for (size_t i = 0; i < shaderNodes.size(); ++i) {
outputLut.insert({ shaderNodes[i].GetOutput(0), i });
}
// get the sink node
size_t sinkNodeIdx = -1;
for (size_t i = 0; i < shaderNodes.size(); ++i) {
if (shaderNodes[i].GetOutput(0)->begin() == shaderNodes[i].GetOutput(0)->end()) {
if (sinkNodeIdx != -1) {
throw InvalidArgumentException("Invalid graph: multiple sink nodes.");
}
sinkNodeIdx = i;
}
}
if (sinkNodeIdx == -1) {
throw InvalidArgumentException("Invalid graph: no sink nodes, contains circle.");
}
// run backwards DFS from sink node
// - get topological order
// - get list of free params
struct FreeParam {
size_t node, input;
std::string name;
};
std::vector<size_t> topologicalOrder;
std::vector<bool> visited(shaderNodes.size(), false);
std::vector<FreeParam> freeParams;
auto VisitNode = [&](size_t node, auto& self) {
if (visited[node]) {
return;
}
visited[node] = true;
for (int i = 0; i < shaderNodes[node].GetNumInputs(); ++i) {
auto* input = shaderNodes[node].GetInput(i);
if (input->GetLink() != nullptr) {
auto* linkOutput = input->GetLink();
size_t linkNode = outputLut[linkOutput];
self(linkNode, self);
}
else {
std::stringstream ss;
ss << m_nodes[node]->GetName() << "__" << shaderNodeParams[node][i].name;
static_cast<InputPort<std::string>*>(input)->Set(ss.str());
freeParams.push_back({ node, (size_t)i, ss.str() });
}
}
std::stringstream ss;
ss << "main_" << topologicalOrder.size();
shaderNodes[node].SetFunctionName(ss.str());
functions[node] = std::regex_replace(functions[node], std::regex("main"), ss.str());
topologicalOrder.push_back(node);
shaderNodes[node].SetFunctionReturn(GetParameterString(shaderNodeReturns[node]));
};
// attach final input port to sink node, and update according to topological order
InputPort<std::string> finalCodePort;
shaderNodes[sinkNodeIdx].GetOutput(0)->Link(&finalCodePort);
VisitNode(sinkNodeIdx, VisitNode);
for (auto idx : topologicalOrder) {
shaderNodes[idx].Update();
}
// assemble resulting code
std::stringstream finalCode;
// sub-functions
for (auto node : topologicalOrder) {
finalCode << functions[node] << "\n";
}
finalCode << "\n\n";
// signature
std::string returnType = GetParameterString(shaderNodeReturns[*--topologicalOrder.end()]);
finalCode << returnType << " main(";
bool firstParam = true;
for (auto& p : freeParams) {
if (!firstParam)
finalCode << ", ";
finalCode << GetParameterString(shaderNodeParams[p.node][p.input].type) << " ";
finalCode << p.name;
firstParam = false;
}
finalCode << ") {\n";
finalCode << "\n";
// preambles
for (auto idx : topologicalOrder) {
finalCode << " " << shaderNodes[idx].GetPreamble() << "\n";
}
finalCode << "\n";
// return statement
finalCode << "return " << finalCodePort.Get() << ";\n";
finalCode << "}\n";
m_source = finalCode.str();
}
void getNextPose(brics_actuator::CartesianPose* curr_next)
{
// cout<<"curr: "<<curr_next->orientation.x<<","<<curr_next->orientation.y<<","<<curr_next->orientation.z<<","<<curr_next->orientation.w<<","<<endl;
// relative_position_port->read(relative_position);
measured_position_port->read(measured_position);
// curr_next->position.x += 0.001;
dummy_x=(measured_position.position.x - goal[0])*Kp+Ki*int_x;
dummy_y=(measured_position.position.y - goal[1])*Kp+Ki*int_y;
dummy_z=(measured_position.position.z - goal[2])*Kp+Ki*int_z;
dummy_ox = (-curr_next->orientation.x + goal[3])*Kp+Ki*int_ox;
dummy_oy = (-curr_next->orientation.y + goal[4])*Kp+Ki*int_oy;
dummy_oz = (-curr_next->orientation.z + goal[5])*Kp+Ki*int_oz;
dummy_ow = (-curr_next->orientation.w + goal[6])*Kp+Ki*int_ow;
if (abs(dummy_x) > STEP_POS) {
dummy_x=SIGN(dummy_x)*STEP_POS;
}
if (abs(dummy_y )> STEP_POS) {
dummy_y=SIGN(dummy_y)*STEP_POS;
}
if (abs(dummy_z )> STEP_POS) {
dummy_z=SIGN(dummy_z)*STEP_POS;
}
if (abs(dummy_ox )> STEP_OR) {
dummy_ox=SIGN(dummy_ox)*STEP_OR;
}
if (abs(dummy_oy )> STEP_OR) {
dummy_oy=SIGN(dummy_oy)*STEP_OR;
}
if (abs(dummy_oz )> STEP_OR) {
dummy_oz=SIGN(dummy_oz)*STEP_OR;
}
if (abs(dummy_ow )> STEP_OR) {
dummy_ow=SIGN(dummy_ow)*STEP_OR;
}
// log(Error)<<sqrt( )<<endlog();
// cout<<sqrt( (measured_position.position.x-goal[0]) *(measured_position.position.x-goal[0]) + (measured_position.position.y-goal[1]) *(measured_position.position.y -goal[1]) + (measured_position.position.z-goal[2]) *(measured_position.position.z-goal[2]) )<<endl;
if(sqrt((measured_position.position.x-goal[0]) *(measured_position.position.x-goal[0]) + (measured_position.position.y-goal[1]) *(measured_position.position.y -goal[1]) + (measured_position.position.z-goal[2]) *(measured_position.position.z-goal[2])) < 0.3)
{
// ((Property<tFriKrlData>)getOwner()->getPeer("KULRobot")->properties()->getProperty("toKRL")).value().intData[1]=3;
// ((Property<tFriKrlData>)getPeer("KULRobot")->getProperty("toKRL")).value().intData[1]=3;
if(this->getOwner()->hasPeer("KULRobot")){
this->getOwner()->getPeer("KULRobot")->properties()->getPropertyType<tFriKrlData>("toKRL")->value().intData[1]=3;
}
log(Warning)<<"Grasp position reached."<<endlog();
}
if(sqrt(dummy_x*dummy_x + dummy_y*dummy_y + dummy_z*dummy_z) < 0.01)
{
// ((Property<tFriKrlData>)getOwner()->getPeer("KULRobot")->properties()->getProperty("toKRL")).value().intData[1]=3;
// ((Property<tFriKrlData>)getPeer("KULRobot")->getProperty("toKRL")).value().intData[1]=3;
if(this->getOwner()->hasPeer("KULRobot")){
this->getOwner()->getPeer("KULRobot")->properties()->getPropertyType<tFriKrlData>("toKRL")->value().intData[1]=3;
}
log(Warning)<<"Grasp position reached."<<endlog();
}
// cout<<dummy_x<<","<<dummy_y<<","<<dummy_z<<","<<dummy_ox<<","<<dummy_oy<<","<<dummy_oz<<endl;
curr_next->position.x +=dummy_x; //+Kd*(measured_position.position.x - 0.02427-prev_error_x)/0.005;
curr_next->position.y +=dummy_y; //+Kd*(measured_position.position.y - 0.02427-prev_error_y)/0.005;
curr_next->position.z += dummy_z;//+Kd*(measured_position.position.z - 0.02427-prev_error_z)/0.005;
curr_next->orientation.x += dummy_ox;
curr_next->orientation.y += dummy_oy;
curr_next->orientation.z += dummy_oz;
curr_next->orientation.w += dummy_ow;
int_x=int_x+dummy_x*0.005;
int_y=int_y+dummy_y*0.005;
int_z=int_z+dummy_z*0.005;
int_ox=int_ox+dummy_ox*0.005;
int_oy=int_oy+dummy_oy*0.005;
int_oz=int_oz+dummy_oz*0.005;
int_ow=int_ow+dummy_ow*0.005;
prev_error_x=dummy_x;
prev_error_y=dummy_y;
prev_error_z=dummy_z;
// (*curr_next) = measured_position;
// PID_return=calcDesiredIncremenPID(relative_position.position.x,x,dt);
// next->position.x=measured_position.position.x+PID_return.desInc;
// x.integral=PID_return.integral;
// x.prev_error=relative_position.position.x;
//
// PID_return=calcDesiredIncremenPID(relative_position.position.y,y,dt);
// next->position.y=measured_position.position.y+PID_return.desInc;
// y.integral=PID_return.integral;
// y.prev_error=relative_position.position.y;
//
// PID_return=calcDesiredIncremenPID(relative_position.position.z,z,dt);
// next->position.z=measured_position.position.z+PID_return.desInc;
// z.integral=PID_return.integral;
// z.prev_error=relative_position.position.z;
//
// PID_return=calcDesiredIncremenPID(relative_position.orientation.wx,wx,dt);
// next->orientation.wx=measured_position.orientation.wx+PID_return.desInc;
// wx.integral=PID_return.integral;
// wx.prev_error=relative_position.orientation.wx;
//
// PID_return=calcDesiredIncremenPID(relative_position.orientation.wy,wy,dt);
// next->orientation.wy=measured_position.orientation.wy+PID_return.desInc;
// wy.integral=PID_return.integral;
// wy.prev_error=relative_position.orientation.wy;
//
// PID_return=calcDesiredIncremenPID(relative_position.orientation.wz,wz,dt);
// next->orientation.wz=measured_position.orientation.wz+PID_return.desInc;
// wz.integral=PID_return.integral;
// wz.prev_error=relative_position.orientation.wz;
//
// PID_return=calcDesiredIncremenPID(relative_position.orientation.ww,ww,dt);
// next->orientation.ww=measured_position.orientation.ww+PID_return.desInc;
// ww.integral=PID_return.integral;
// ww.prev_error=relative_position.orientation.ww;
}
/**
* Creates the connection end that represents the output and attach
* it to the input.
* @param port The start point.
* @param output_id Each connection must be identified by an ID that
* represents the other end. This id is passed to the input port \a port.
* @return
*/
ConnOutputEndpoint(InputPort<T>* port, ConnID* output_id )
: port(port), cid(output_id->clone())
{
port->addConnection(output_id, this );
}
void CorbaTest::testPortDisconnected()
{
BOOST_CHECK( !mo1->connected() );
BOOST_CHECK( !mi2->connected() );
}
void CorbaMQueueIPCTest::testPortDisconnected()
{
BOOST_CHECK( !mw1->connected() );
BOOST_CHECK( !mr1->connected() );
}
