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 );
}
void updateHook() {
dummyInPort.read(dataVar);
dummyOutPort.write(dataVar);
dataInPort.read(dataVar);
dataOutPort.write(dataVar);
//log(Info) << "Worker relaying data!" << endlog();
}
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);
// 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 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;
*/
}
void updateHook() {
double input_sample;
if ( input.read(input_sample) ) {
current += this->engine()->getActivity()->getPeriod()*input_sample /inertia.value();
}
output.write( current );
}
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 );
}
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;
}
bool evaluate() const
{
return port->read(mvalue, false) == NewData;
}
