Exemple #1
0
/*
 * This tutorial demonstrates simple sending of messages over the ROS system.
 */
int main(int argc, char **argv)
{
	//
  /*
   * The ros::init() function needs to see argc and argv so that it can perform
   * any ROS arguments and name remapping that were provided at the command line. For programmatic
   * remappings you can use a different version of init() which takes remappings
   * directly, but for most command-line programs, passing argc and argv is the easiest
   * way to do it.  The third argument to init() is the name of the node.
   *
   * You must call one of the versions of ros::init() before using any other
   * part of the ROS system.
   */
  ros::init(argc, argv, "dasl_mocap");
  /*
   * NodeHandle is the main access point to communications with the ROS system.
   * The first NodeHandle constructed will fully initialize this node, and the last
   * NodeHandle destructed will close down the node.
   */
  ros::NodeHandle n;

  /**
   * The advertise() function is how you tell ROS that you want to
   * publish on a given topic name. This invokes a call to the ROS
   * master node, which keeps a registry of who is publishing and who
   * is subscribing. After this advertise() call is made, the master
   * node will notify anyone who is trying to subscribe to this topic name,
   * and they will in turn negotiate a peer-to-peer connection with this
   * node.  advertise() returns a Publisher object which allows you to
   * publish messages on that topic through a call to publish().  Once
   * all copies of the returned Publisher object are destroyed, the topic
   * will be automatically unadvertised.
   *
   * The second parameter to advertise() is the size of the message queue
   * used for publishing messages.  If messages are published more quickly
   * than we can send them, the number here specifies how many messages to
   * buffer up before throwing some away.
   */
  //ros::Publisher chatter_pub = n.advertise<geometry_msgs::Transform>("servo", 1000);
  //ros::Publisher chatter_pub = n.advertise<mocap::pc2quadcopter>("pc2quadcopter", 1000);
  //ros::Rate loop_rate(100);

  //ros::init(argc, argv, "mocap", ros::init_options::NoSigintHandler );

  /**
   * A count of how many messages we have sent. This is used to create
   * a unique string for each message.
   */
  //Joystick joy;
///////////////////////////////////////////////////////////////////////////
  MOCAPSocket Socket;
///////////////////////////////////////////////////////////////////////////

  static tf::TransformBroadcaster br;

  int count = 0;
  //float thrust=1000;
  //float roll=0.0;
  //float pitch=0.0;

  while (ros::ok())
  {
	  //mocap::pc2quadcopter msg;
   if(Socket.Read()>0)
   {
    /**
     * This is a message object. You stuff it with data, and then publish it.
     */
	ros::Time timestamp( ros::Time::now() );

	tf::Transform transform;
	// Translate mocap data from mm --> m to be compatible with rviz
	transform.setOrigin( tf::Vector3(Socket.rigidBody[0].x / 1000.0f,Socket.rigidBody[0].z / 1000.0f,Socket.rigidBody[0].y / 1000.0f ) );
			    
	// Switch y and z axes due to calibration of mocap

	tf::Quaternion q( Socket.rigidBody[0].qx, Socket.rigidBody[0].qz, Socket.rigidBody[0].qy, Socket.rigidBody[0].qw ) ;
			    
	transform.setRotation(q.inverse());		// Handle 

	int rigid_body_id = abs(Socket.rigidBody[0].ID);
	const char* rigid_body_name = "OBJECT"; //mocap_model[rigid_body_id];
	br.sendTransform(tf::StampedTransform(transform, timestamp, "base_link", std::string( rigid_body_name ) ));

	//tf::TransformBroadcaster msg;
	
	//mocap::pc2quadcopter msg;

	//msg.orientation.x=Socket.rigidBody[0].roll*3.14/180;
	//msg.orientation.y=Socket.rigidBody[0].pitch*3.14/180;
	//msg.orientation.z=Socket.rigidBody[0].yaw*3.14/180;

	//msg.position.x= -Socket.rigidBody[0].z;
	//msg.position.y= Socket.rigidBody[0].x;
	//msg.position.z=Socket.rigidBody[0].y;

	//msg.speed.x=-(Socket.rigidBody[0].z-Socket.rigidBody[0].z_old)/20;
	//msg.speed.y=(Socket.rigidBody[0].x-Socket.rigidBody[0].x_old)/20;
	//msg.speed.z=(Socket.rigidBody[0].y-Socket.rigidBody[0].y_old)/20;

	//msg.pitch=pitch;
	//msg.roll=roll;
    	//chatter_pub.publish(msg);
    }
    //ROS_INFO("%s", msg.data.c_str());

    /**
     * The publish() function is how you send messages. The parameter
     * is the message object. The type of this object must agree with the type
     * given as a template parameter to the advertise<>() call, as was done
     * in the constructor above.
     */

    ros::spinOnce();

    //loop_rate.sleep();
    ++count;
  }

  return 0;
}
/*
 * This tutorial demonstrates simple sending of messages over the ROS system.
 */
int main(int argc, char **argv)
{

  char * trackable = NULL;
  FILE * trackables;
  char a[15], drones[10][26],drones_vel[10][30];
  char default_name[50] = "/Optitrack";
  int DronesID[10];
  char default_name_vel[50] = "/Optitrack_vel";
  int i = 0, k = 0, j = 0, index = -1;
  int rate = 20;
  float r = 1;
  int counter = 0;
  //
  /*
   * The ros::init() function needs to see argc and argv so that it can perform
   * any ROS arguments and name remapping that were provided at the command line. For programmatic
   * remappings you can use a different version of init() which takes remappings
   * directly, but for most command-line programs, passing argc and argv is the easiest
   * way to do it.  The third argument to init() is the name of the node.
   *
   * You must call one of the versions of ros::init() before using any other
   * part of the ROS system.
   */

  argc--; argv++;
  while( argc && *argv[0] == '-' )
  {
    if( !strcmp(*argv, "-trackables") && ( argc > 1 ) )
    {
      trackable = *(argv+1);
      printf("Using custom drone names from file - %s\n",trackable);
      argc--; argv++;
    }
    argc--; argv++;
  }

  trackables=fopen(trackable, "r");
  if ((trackables==NULL) and (trackable!=NULL)) printf("Can't open file %s\n", trackable);
  if (trackable==NULL) printf("Using default topic name.\n");
  printf("----------------------------------------------\n");

  ros::init(argc, argv, "NatNet_Talker");

  /*
  * NodeHandle is the main access point to communications with the ROS system.
  * The first NodeHandle constructed will fully initialize this node, and the last
  * NodeHandle destructed will close down the node.
  */
  ros::NodeHandle n;

  if (trackables!=NULL)
  {
    while ((fscanf(trackables, "%s",a)==1) and i<=10)
    {
      if (k % 2 == 0)
      {
        strcpy(drones[i], a);
        strcat(drones[i], default_name);
        printf("Creating topic %s\n",drones[i]);
        strcpy(drones_vel[i], a);
        strcat(drones_vel[i], default_name_vel);
        printf("Creating topic %s\n",drones_vel[i]);
      }
      else
      {
        DronesID[i] = atoi(a);
        ++i;
      }
      k++;
    }
  }
  else 
  {
    strcpy(drones[i], default_name);
    strcpy(drones_vel[i], default_name_vel);
    i = 1;
  }

  /**
  * The advertise() function is how you tell ROS that you want to
  * publish on a given topic name. This invokes a call to the ROS
  * master node, which keeps a registry of who is publishing and who
  * is subscribing. After this advertise() call is made, the master
  * node will notify anyone who is trying to subscribe to this topic name,
  * and they will in turn negotiate a peer-to-peer connection with this
  * node.  advertise() returns a Publisher object which allows you to
  * publish messages on that topic through a call to publish().  Once
  * all copies of the returned Publisher object are destroyed, the topic
  * will be automatically unadvertised.
  *
  * The second parameter to advertise() is the size of the message queue
  * used for publishing messages.  If messages are published more quickly
  * than we can send them, the number here specifies how many messages to
  * buffer up before throwing some away.
  */

  ros::Publisher *chatter_pub = new ros::Publisher[i];
  ros::Publisher *chatter_pub_vel = new ros::Publisher[i];
  geometry_msgs::PoseStamped *msg = new geometry_msgs::PoseStamped[i];
  geometry_msgs::TwistStamped *msg_old = new geometry_msgs::TwistStamped[i];
  geometry_msgs::TwistStamped *msg_old2 = new geometry_msgs::TwistStamped[i];
  geometry_msgs::TwistStamped *msg_vel = new geometry_msgs::TwistStamped[i];

  for (k=0;k<i;k++)
  {
    chatter_pub[k] = n.advertise<geometry_msgs::PoseStamped>(drones[k], 1);
    chatter_pub_vel[k] = n.advertise<geometry_msgs::TwistStamped>(drones_vel[k], 1);
  }

///////////////////////////////////////////////////////////////////////////
  MOCAPSocket Socket;
///////////////////////////////////////////////////////////////////////////


  ros::Rate loop_rate(rate);

  while (ros::ok())
  {
    //mocap::pc2quadcopter msg;
    if(Socket.Read()>0)
    {
      /**
      * This is a message object. You stuff it with data, and then publish it.
      */
      //mocap::pc2quadcopter msg;
      //msg.rotation.x=Socket.rigidBody[0].qx;
      //msg.rotation.y=Socket.rigidBody[0].qy;
      //msg.rotation.z=Socket.rigidBody[0].qz;
      //msg.rotation.w=Socket.rigidBody[0].qw;
      //msg.translation.x
      //joy.Refresh();

      for (k=0;k<i;k++)
      {

        index = -1;
        for (j=0;j<Socket.NmbrOfRigidBody;j++)
        {
          if (Socket.rigidBodyID[j] == DronesID[k])
          {
            index = j;
            break;
          }
        }

        if (index!=-1)
        {
          if (Socket.rigidBody[index].pitch!=0 && Socket.rigidBody[index].roll!=0 && Socket.rigidBody[index].yaw!=0 && Socket.rigidBody[index].z!=0)
          {
            msg_old2[k] = msg_old[k];
            msg_old[k] = msg_vel[k];
            msg_vel[k].twist.linear.x = (Socket.rigidBody[index].z - msg[k].pose.position.x)*rate;
            msg_vel[k].twist.linear.y = (Socket.rigidBody[index].x - msg[k].pose.position.y)*rate;
            msg_vel[k].twist.linear.z = (Socket.rigidBody[index].y - msg[k].pose.position.z)*rate;
            msg_vel[k].twist.angular.z = (Socket.rigidBody[index].yaw*3.14/180 - msg[k].pose.orientation.z)*rate;

            //msg[k].vel.position.x = LowPassFilter((Socket.rigidBody[k].z - msg[k].pose.pose.position.x)*rate, msg[k].vel.position.x, r);
            //msg[k].vel.position.y = LowPassFilter((Socket.rigidBody[k].x - msg[k].pose.pose.position.y)*rate, msg[k].vel.position.y, r);
            //msg[k].vel.position.z = LowPassFilter((Socket.rigidBody[k].y - msg[k].pose.pose.position.z)*rate, msg[k].vel.position.z, r);
            //msg[k].vel.orientation.z = LowPassFilter((Socket.rigidBody[k].yaw*3.14/180 - msg[k].pose.pose.orientation.z)*rate, msg[k].vel.orientation.z, r);

            msg_vel[k].twist.linear.x = msg_vel[k].twist.linear.x *0.4 + msg_old[k].twist.linear.x*0.3+ msg_old2[k].twist.linear.x*0.3;
            msg_vel[k].twist.linear.y = msg_vel[k].twist.linear.y*0.4 + msg_old[k].twist.linear.y*0.3+ msg_old2[k].twist.linear.y*0.3;
            msg_vel[k].twist.linear.z = msg_vel[k].twist.linear.z*0.4 + msg_old[k].twist.linear.z*0.3+ msg_old2[k].twist.linear.z*0.3;
            msg_vel[k].twist.angular.z = msg_vel[k].twist.angular.z*0.4 + msg_old[k].twist.angular.z*0.3 +msg_old2[k].twist.angular.z*0.3;

            if (abs(msg_old[k].twist.linear.x - msg_vel[k].twist.linear.x) > 0.3) msg_vel[k].twist.linear.x = 0;
            if (abs(msg_old[k].twist.linear.y - msg_vel[k].twist.linear.y) > 0.3) msg_vel[k].twist.linear.y = 0;
            if (abs(msg_old[k].twist.linear.z - msg_vel[k].twist.linear.z) > 0.3) msg_vel[k].twist.linear.z = 0;
            if (abs(msg_old[k].twist.angular.z - msg_vel[k].twist.angular.z) > 0.3) msg_vel[k].twist.angular.z = 0;

            msg[k].pose.orientation.x=Socket.rigidBody[index].pitch*3.14/180;
            //msg.translation.y=Socket.rigidBody[0].y;
            msg[k].pose.orientation.y=Socket.rigidBody[index].roll*3.14/180;
            //msg.translation.z=Socket.rigidBody[0].z;
            msg[k].pose.orientation.z=Socket.rigidBody[index].yaw*3.14/180;
            //msg.orientation.w=(Socket.rigidBody[0].x-Socket.rigidBody[0].x_old)/20;
            //msg.position.x= (joy.axis1_-500)*3.14/4500;
            //msg.position.y= -(joy.axis2_-500)*3.14/4500;
            msg[k].pose.position.x= Socket.rigidBody[index].z;
            msg[k].pose.position.y= Socket.rigidBody[index].x;
            msg[k].pose.position.z= Socket.rigidBody[index].y;
            msg[k].header.stamp =ros::Time::now();
            //msg.speed.x=-(Socket.rigidBody[0].z-Socket.rigidBody[0].z_old)/20;
            //msg.speed.y=(Socket.rigidBody[0].x-Socket.rigidBody[0].x_old)/20;
            //msg.speed.z=(Socket.rigidBody[0].y-Socket.rigidBody[0].y_old)/20;
          }
          chatter_pub[k].publish(msg[k]);
          chatter_pub_vel[k].publish(msg_vel[k]);
        }
      }        
    }
    //ROS_INFO("%s", msg.data.c_str());

    ros::spinOnce();

    loop_rate.sleep();
  }

  delete [] chatter_pub;
  delete [] msg;

  if (trackables!=NULL) fclose(trackables);

  return 0;
}
void processMocapData( const char** mocap_model )
{
  //UdpMulticastSocket multicast_client_socket( LOCAL_PORT, MULTICAST_IP );
		
  MOCAPSocket Socket;

  static tf::TransformBroadcaster br;

  //ushort payload;
  //MoCapDataFormat format;
  while( true )
  {	
    //int numberOfPackets = 0;
    //bool packetread = false;

    //int trials = 0;

    //while( !packetread && trials < 100 )
    //{
      //int numBytes = 0;

      //do
      //{
	// Receive data form mocap device
	//numBytes = multicast_client_socket.recv();

	// Parse mocap data
	//if( numBytes > 0 )
	//{
	  //const char* buffer = multicast_client_socket.getBuffer();
	  //unsigned short header = *((unsigned short*)(&buffer[0]));
					
	  // Look for the beginning of a NatNet package
	  //if (header == 7)
	  //{
	    //payload = *((ushort*) &buffer[2]);
	    //format.parse(buffer,payload);
	    //packetread = true;
	    //numberOfPackets++;
	  //}
	  // else skip packet
	//}


      //} while( numBytes > 0 );

      // Don't try again immediately
      //if( !packetread )
      //{
	//usleep( 10 );
	//trials++;
      //}
    //}
		
    // Once a mocap package has been received and parsed, publish the data using tf
    if(Socket.Read()>0)
    //if( packetread )
    {
      ros::Time timestamp( ros::Time::now() );
			
      //for( int i = 0; i < format.model[0].numRigidBodies; i++ )
      //{

	tf::Transform transform;
	// Translate mocap data from mm --> m to be compatible with rviz

	// Change y and z due to mocap calibration

	transform.setOrigin( tf::Vector3(-Socket.rigidBody[0].x / 1000.0f,Socket.rigidBody[0].z / 1000.0f,Socket.rigidBody[0].y / 1000.0f ) );

	// Change y and z due to mocap calibration
			    
	tf::Quaternion q( Socket.rigidBody[0].qx, Socket.rigidBody[0].qz, Socket.rigidBody[0].qy, Socket.rigidBody[0].qw ) ;
			    
	transform.setRotation(q.inverse());		// Handle 

	int rigid_body_id = abs(Socket.rigidBody[0].ID);
	const char* rigid_body_name = "OBJECT"; //mocap_model[rigid_body_id];
	br.sendTransform(tf::StampedTransform(transform, timestamp, "base_link", std::string( rigid_body_name ) ));

      //}
    }
  }
}