Ejemplo n.º 1
0
int main(int argc, char **argv)
{
	ros::init(argc, argv, "dummy_startup");
	ros::NodeHandle nh;
	ros::NodeHandle p_nh("~");
	client = nh.serviceClient<aero_srr_msgs::StateTransitionRequest>("/aero/supervisor/state_transition_request");
	sub= nh.subscribe("aero/software_stop", 1, stopCB);
	
	while(!got_free&&ros::ok())
	{
		ros::spinOnce();
	}
	return 1;
}
Ejemplo n.º 2
0
ArCalibration::ArCalibration(void)
{
	ros::NodeHandle p_nh("~");
	
	nh_.param<int>("marker_id", marker_id_, 0);
	
	// initialize tag&arm position
	tag_pos_.x = 0.0; tag_pos_.y = 0.0; tag_pos_.z = 0.0;
	arm_pos_.position.x = 0.0; arm_pos_.position.y = 0.0; arm_pos_.position.z = 0.0;
	arm_pos_.orientation.x = 0.0; arm_pos_.orientation.y = 0.0; arm_pos_.orientation.z = 0.0; arm_pos_.orientation.w = 1.0; 
    
	ar_marker_sub_ = nh_.subscribe("/ar_pose_marker", 1, &ArCalibration::arMarkerCallback, this);
	arm_pos_sub_ = nh_.subscribe("/jaco_arm_driver/out/tool_position", 1, &ArCalibration::armPosCallback, this);
	trigger_sub_ = nh_.subscribe("/joy", 1, &ArCalibration::joyCallback, this);
	
	tag_pub_ = nh_.advertise<geometry_msgs::Point>("output_tag_position", 1);
	arm_pub_ = nh_.advertise<geometry_msgs::Pose>("output_arm_position", 1);


	ROS_INFO(" READY TO RECEIVE THE DATA FOR CALIBRATION");
}
int main(int argc, char ** argv)
{
  //Initialize ROS
  ros::init(argc, argv, "tf_echo", ros::init_options::AnonymousName);

  // Allow 2 or 3 command line arguments
  if (argc < 3 || argc > 4)
  {
    printf("Usage: tf_echo source_frame target_frame [echo_rate]\n\n");
    printf("This will echo the transform from the coordinate frame of the source_frame\n");
    printf("to the coordinate frame of the target_frame. \n");
    printf("Note: This is the transform to get data from target_frame into the source_frame.\n");
    printf("Default echo rate is 1 if echo_rate is not given.\n");
    return -1;
  }

  ros::NodeHandle nh;

  double rate_hz;
  if (argc == 4)
  {
    // read rate from command line
    rate_hz = atof(argv[3]);
  }
  else
  {
    // read rate parameter
    ros::NodeHandle p_nh("~");
    p_nh.param("rate", rate_hz, 1.0);
  }
  ros::Rate rate(rate_hz);

  //Instantiate a local listener
  echoListener echoListener;


  std::string source_frameid = std::string(argv[1]);
  std::string target_frameid = std::string(argv[2]);

  // Wait for up to one second for the first transforms to become avaiable. 
  echoListener.tf.waitForTransform(source_frameid, target_frameid, ros::Time(), ros::Duration(1.0));

  //Nothing needs to be done except wait for a quit
  //The callbacks withing the listener class
  //will take care of everything
  while(nh.ok())
    {
      try
      {
        tf::StampedTransform echo_transform;
        echoListener.tf.lookupTransform(source_frameid, target_frameid, ros::Time(), echo_transform);
        std::cout.precision(3);
        std::cout.setf(std::ios::fixed,std::ios::floatfield);
        std::cout << "At time " << echo_transform.stamp_.toSec() << std::endl;
        double yaw, pitch, roll;
        echo_transform.getBasis().getRPY(roll, pitch, yaw);
        tf::Quaternion q = echo_transform.getRotation();
        tf::Vector3 v = echo_transform.getOrigin();
        std::cout << "- Translation: [" << v.getX() << ", " << v.getY() << ", " << v.getZ() << "]" << std::endl;
        std::cout << "- Rotation: in Quaternion [" << q.getX() << ", " << q.getY() << ", " 
                  << q.getZ() << ", " << q.getW() << "]" << std::endl
                  << "            in RPY (radian) [" <<  roll << ", " << pitch << ", " << yaw << "]" << std::endl
                  << "            in RPY (degree) [" <<  roll*180.0/M_PI << ", " << pitch*180.0/M_PI << ", " << yaw*180.0/M_PI << "]" << std::endl;

        //print transform
      }
      catch(tf::TransformException& ex)
      {
        std::cout << "Failure at "<< ros::Time::now() << std::endl;
        std::cout << "Exception thrown:" << ex.what()<< std::endl;
        std::cout << "The current list of frames is:" <<std::endl;
        std::cout << echoListener.tf.allFramesAsString()<<std::endl;
        
      }
      rate.sleep();
    }

  return 0;
};
int main (int argc, char **argv)
{	
	RT_TASK *task;
	M3Sds * sys;
	int cntr=0;
	
	rt_allow_nonroot_hrt();
	
	/*ros::init(argc, argv, "base_controller"); // initialize ROS node
  	ros::AsyncSpinner spinner(1); // Use 1 thread - check if you actually need this for only publishing
  	spinner.start();
        ros::NodeHandle root_handle;*/
	
	ros::init(argc, argv, "led_controller", ros::init_options::NoSigintHandler); // initialize ROS node
	ros::AsyncSpinner spinner(1); // Use 1 thread - check if you actually need this for only publishing
	spinner.start();
        ros::NodeHandle root_handle;
	ros::NodeHandle p_nh("~");
	
	cmd_sub_g = root_handle.subscribe<shm_led_mouth::LEDMatrixCmd>("/led_matrix_command", 1, &commandCallback);
	
	
	signal(SIGINT, endme);

	if (sys = (M3Sds*)rt_shm_alloc(nam2num(MEKA_LED_SHM),sizeof(M3Sds),USE_VMALLOC))
		printf("Found shared memory starting shm_led_mouth_controller.");
	else
	{
		printf("Rtai_malloc failure for %s\n",MEKA_LED_SHM);
		return 0;
	}

	rt_allow_nonroot_hrt();
	/*if (!(task = rt_task_init_schmod(nam2num("TSHM"), RT_TASK_PRIORITY, 0, 0, SCHED_FIFO, 0xF)))
	{
		rt_shm_free(nam2num(TORQUE_SHM));
		printf("Cannot init the RTAI task %s\n","TSHM");
		return 0;
	}*/
	hst=rt_thread_create((void*)rt_system_thread, sys, 10000);
	usleep(100000); //Let start up
	if (!sys_thread_active)
	{
		rt_task_delete(task);
		rt_shm_free(nam2num(MEKA_LED_SHM));
		printf("Startup of thread failed.\n",0);
		return 0;
	}
	while(!end)
	{		
		usleep(250000);
		
	}
	printf("Removing RT thread...\n",0);
	sys_thread_end=1;
	//rt_thread_join(hst);
	usleep(1250000);
	if (sys_thread_active)printf("Real-time thread did not shutdown correctly\n");
	//rt_task_delete(task);
	rt_shm_free(nam2num(MEKA_LED_SHM));
	ros::shutdown();	
	return 0;
}