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