/*
* 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 ) ));
//}
}
}
}