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;
}