void CCBuffer::writeDouble(double data)
{
#if MEMORYTYPE_REVERSE
DO_ASSIGN(char p_data[sizeof(double)], {0});
memcpy(p_data, (char*)(QZ(data)), sizeof(double));
std::reverse(QZ(p_data[0]), QZ(p_data[sizeof(double)]));
writeData(p_data, sizeof(double));
#else
DO_ASSIGN(char* p_data, (char*)(QZ(data)));
writeData(p_data, sizeof(double));
#endif
}
void Buffer::writeFloat(float data)
{
#if MEMORYTYPE_REVERSE
DO_ASSIGN(char p_data[sizeof(float)], {0});
memcpy(p_data, (char*)(QZ(data)), sizeof(float));
std::reverse(QZ(p_data[0]), QZ(p_data[sizeof(float)]));
writeData(p_data, sizeof(float));
#else
DO_ASSIGN(char* p_data, (char*)(QZ(data)));
writeData(p_data, sizeof(float));
#endif
}
int main(int argc, char** argv)
{
char port[] = "27015";
char ip[] = "10.0.0.67";
int numbytes;
char buf[MAXDATASIZE];
// Get input parameters
for(int a=0; a < argc; a++)
{
if( strcmp( argv[a], "--ip" ) == 0 )
{
strcpy(ip, argv[a+1]);
}
if( strcmp( argv[a], "--port" ) == 0 )
{
strcpy(port, argv[a+1]);
}
}
TCP myClient = TCP(port, ip);
ros::init(argc, argv, "joints");
ros::NodeHandle n;
std::string receivedString;
std::string attribute;
std::stringstream ss;
std::istringstream iss;
tf::TransformBroadcaster br;
tf::Transform transform;
ros::Rate loop_rate(30);
if( myClient.Connect() == 0 )
{
// Receive data
numbytes = recv(myClient.s,buf,MAXDATASIZE-1,0);
while (numbytes != 0 && ros::ok())
{
numbytes = recv(myClient.s,buf,MAXDATASIZE-1,0);
buf[numbytes]='\0';
receivedString.assign(buf);
// debug
// std::cout << "Ahoy! Received " << numbytes << " bytes. " << receivedString << std::endl;
std::istringstream iss(receivedString);
std::vector<double> x (25, 0.0);
std::vector<double> y (25, 0.0);
std::vector<double> z (25, 0.0);
std::vector<std::vector<double> > X (6, x);
std::vector<std::vector<double> > Y (6, y);
std::vector<std::vector<double> > Z (6, z);
std::vector<double> qx (25, 0.0);
std::vector<double> qy (25, 0.0);
std::vector<double> qz (25, 0.0);
std::vector<double> qw (25, 1.0);
std::vector<std::vector<double> > QX (6, qx);
std::vector<std::vector<double> > QY (6, qy);
std::vector<std::vector<double> > QZ (6, qz);
std::vector<std::vector<double> > QW (6, qw);
int j;
int count = 6;
int id;
std::vector<int> foundIdx;
std::string frame;
std::string joint;
/* code */
while(!iss.eof())
{
// joint = "joint_";
iss >> attribute;
frame = "person_";
if( strcmp(attribute.c_str(), "count:") == 0 )
{
iss >> count;
// ROS_INFO("count %d",count);
}
else if( strcmp(attribute.c_str(), "id:") == 0 )
{
iss >> id;
foundIdx.push_back(id);
frame.append(boost::lexical_cast<std::string>(id));
// ROS_INFO("id %s",frame);
// std::cout << id << " " << frame << " ";
}
