/*!
* private function, DO NOT CALL DIRECTLY
*/
void SamgarModule::SendData(DataType type,string port,void* data) {
DataPort* MyTempPort;
list<DataPort*>::iterator ItPort;
string TempName;
bool FoundPort;
TempName="/Port_" + MyName + "_" + port.c_str();
FoundPort=false;
for ( ItPort=PortList.begin() ; ItPort != PortList.end(); ItPort++ ) {
MyTempPort=*ItPort;
if(MyTempPort->getName() == TempName.c_str()) {
FoundPort=true;
break; // we have the port so break the loop
}
}
if(FoundPort==true) {
Bottle& MyBottle = MyTempPort-> prepare();
MyBottle.clear();
switch (type){
case TypeInt: MyBottle.addInt( *((int*) data) ); break;
case TypeDouble: MyBottle.addDouble( *((double*) data) ); break;
case TypeString: MyBottle.addString( ((string*) data)->c_str()); break;
case TypeBottle: MyBottle = *((Bottle*)data); break;
}
MyTempPort->write();
}
}
bool DataPort::insertConnection ( DataConnection* c ) {
// qDebug() << __PRETTY_FUNCTION__;
DataPort* dstItem = static_cast<DataPort*> ( c->dst() );
m_childItems.append ( c );
dstItem->insertReference ( c );
return true;
}
NoiseNetwork* Displace::network() {
// 0. query module for readieness
// a module is ready if all dependencies are sadisfied, if that did not happen
// we can't use it as libnoise could throw an exception or do other weird things...
if (ready()) {
// 1. create a noiseNetwork (if generator module)
NoiseNetwork* noiseNetwork = new NoiseNetwork;
// 2. construct the local module (in this case Perlin)
noise::module::Displace* m_Displace = new noise::module::Displace();
noiseNetwork->addModule(m_Displace);
// 3. assign all properties
// m_Displace->SetOctaveCount (property("OctaveCount"));
//FIXME: implement this
// 4. connect all source modules (in case of a generator module: none) & merge the NoiseNetwork(s)
// foreach(input i)
// m_perlin->SetSourceModule(i->portnumber, i->topLevelModule);
// noiseNetwork->merge(i->network)
// foreach(input m)
// m_perlin->SetControlModule(m->topLevelModule);
// noiseNetwork->merge(m->network)
for (int x=0; x < childCount(); ++x) {
DataAbstractItem* chi = childItems()[x];
if (chi->getObjectType() == DataItemType::PORT) {
DataPort* p = static_cast<DataPort*>(chi);
if (p->PortDirection() == PortDirection::IN || p->PortDirection() == PortDirection::MOD) {
// 1. reconstruct the Connection
DataConnection* c = static_cast<DataConnection*> (p->referenceChildItems().first());
// 2. reconstruct the remote port
DataAbstractItem* abstractPort = c->dst();
// 3. reconstruct the remote module
DataAbstractModule* module = static_cast<DataAbstractModule*> (abstractPort->parent());
// 4. check if it is ready
/*
if (p->PortDirection() == PortDirection::IN) {
NoiseNetwork* n = module->network();
m_Displace->SetSourceModule(p->PortNumber(), n->topLevelModule());
noiseNetwork += n;
}
*/
/*
if (p->PortDirection() == PortDirection::MOD) {
NoiseNetwork* n = module->network();
m_Displace->SetControlModule(n->topLevelModule());
noiseNetwork += n;
}
*/
}
}
}
return noiseNetwork;
}
return NULL;
}
void
fini ()
{
// port_rpc_say_state_.close ();
// port_out_say_.close ();
port_in_ext_motion_det.close ();
port_in_int_motion_det.close ();
Network::fini ();
}
/*!
* DO NOT CALL DIRECTLY
*/
bool SamgarModule::GetDataFromPort(string NameOfPort,int TypeOfData, void* data) {
list<DataPort*>::iterator ItPort;
DataPort *MyTempPort;
bool HaveIfoundPort;
NameOfPort = "/Port_" + MyName + "_" + NameOfPort.c_str();
HaveIfoundPort = false;
for ( ItPort=PortList.begin(); ItPort != PortList.end(); ItPort++ ) {// find the right port loop
MyTempPort=*ItPort;
if(MyTempPort->getName() == NameOfPort.c_str()){
HaveIfoundPort = true;
break;
}// have the data we need so break the loop
}
if ( !HaveIfoundPort )
return false;
if (MyTempPort->isClosed())
return false;
if(MyTempPort->getInputCount() == 0)
return false; // if nothings connected to it
// gets Bottle from the DataPort storage
Bottle MyBottle=MyTempPort->getBottle();
if(MyBottle.isNull())
return false;
if (TypeOfData == TypeInt && MyBottle.get(0).isInt() != false) {
*((int*)data) = MyBottle.get(0).asInt();
return true;
}
else if (TypeOfData == TypeString && MyBottle.get(0).isString() != false) {
*((string*)data) = MyBottle.get(0).asString();
return true;
}
else if(TypeOfData == TypeDouble && MyBottle.get(0).isDouble() != false) {
*((double*)data) = MyBottle.get(0).asDouble();
return true;
}
else if(TypeOfData == TypeBottle) {
*((Bottle*)data) = MyBottle;
return true;
}
return false;
}
void
init ()
{
//ros initializations
n = new ros::NodeHandle ();
pub_exp_state_ = n->advertise<aff_msgs::ExperimentState> ("/experiment_state", 10);
pub_say_ = n->advertise<sound_play::SoundRequest> ("/robotsound", 2);
sub_speech_cmd_ = n->subscribe<aff_msgs::Speech> ("/speech_command", 2, &speechCmdCallback);
pub_aff_labels = n->advertise<aff_msgs::ObjectOfInterest> ("/aff_labels", 10);
exp_state_ = aff_msgs::ExperimentState::ASK_FOR_ACTION;
// exp_state_ = aff_msgs::ExperimentState::ACTION;
bridge_ = new sensor_msgs::CvBridge ();
//yarp initializations
Network::init ();
if (USE_EXT_MOTION_DETECTION)
{
port_in_ext_motion_det.useCallback ();
port_in_ext_motion_det.open (port_in_ext_motion_det_name.c_str ());
Network::connect (port_out_ext_motion_det_name.c_str (), port_in_ext_motion_det_name.c_str ());
std::cout << "waiting for an -external motion detection- port to be connected" << std::endl;
while (!Network::isConnected (port_out_ext_motion_det_name.c_str (), port_in_ext_motion_det_name.c_str ())
&& n->ok ())
{
}
std::cout << "An -external motion detection- port is connected, ready for activation data..." << std::endl;
}
if (USE_INT_MOTION_DETECTION)
{
port_in_int_motion_det.useCallback ();
port_in_int_motion_det.open (port_in_int_motion_det_name.c_str ());
Network::connect (port_out_int_motion_det_name.c_str (), port_in_int_motion_det_name.c_str ());
std::cout << "waiting for an -internal motion detection- port to be connected" << std::endl;
while (!Network::isConnected (port_out_int_motion_det_name.c_str (), port_in_int_motion_det_name.c_str ())
&& n->ok ())
{
}
std::cout << "An -internal motion detection- port is connected, ready for activation data..." << std::endl;
}
}
int
main (int argc, char* argv[])
{
//ros initializations
ros::init (argc, argv, "mocap_kinects");
ros::NodeHandle n;
//yarp initializations
Network::init ();
DataPort vzInPort;
vzInPort.useCallback ();
vzInPort.open ("/i:vzListen");
Network::connect ("/vzRawPout", "/i:vzListen");
std::cout << "waiting for mocap to be connected" << std::endl;
while (!tf_calculated && n.ok ())
{
}
std::cout << "tf is calculated" << std::endl;
static tf::TransformBroadcaster br;
ros::Rate r (50);
while (n.ok ())
{
if (avg_tf_left != NULL)
{
br.sendTransform (tf::StampedTransform (*avg_tf_left, ros::Time::now (), "/base_footprint", "/left_camera_link"));
}
if (avg_tf_right != NULL)
{
br.sendTransform (
tf::StampedTransform (*avg_tf_right, ros::Time::now (), "/base_footprint", "/right_camera_link"));
}
ros::spinOnce ();
r.sleep ();
}
Network::fini ();
delete avg_tf_left;
delete avg_tf_right;
return 0;
}
void DataPort::removeChild ( unsigned int index ) {
qDebug() << __PRETTY_FUNCTION__;
if ( m_childItems.size() < index ) {
qDebug() << __PRETTY_FUNCTION__ << "FATAL ERROR: child item not found";
exit( 1 );
}
DataAbstractItem* child = m_childItems[index];
// let's not forget to remove also the reference at the other port
DataConnection* childConnection = static_cast<DataConnection*> (child);
DataPort* dstItem = static_cast<DataPort*> ( childConnection->dst() );
int i = dstItem->referenceChildItems().indexOf(childConnection);
// qDebug() << dstItem->referenceChildItems().size();
if (i == -1) {
qDebug() << __PRETTY_FUNCTION__ << "this should not happen";
exit(1);
}
dstItem->removeReference ( i );
m_childItems.removeAt( index );
delete child;
}