Пример #1
0
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;
  }
}
Пример #2
0
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;
}