bool message::receive(czmqpp::socket& socket)
{
bool requires_reply = false;
czmqpp::message message;
requires_reply = message.receive(socket);
if (requires_reply)
{
const data_stack& parts = message.parts();
if (parts.size() > 0)
{
auto it = parts.begin();
// Extract reply address(es)
if (parts.size() > 2)
{
origin_ = *it;
++it;
}
// Confirm empty delimiter frame
++it;
// Parse request payload from data frame
if (decode_payload(*it))
{
++it;
// Confirm end of message (unnecessary)
BITCOIN_ASSERT(it == parts.end());
}
}
}
return requires_reply;
}
//funzione che riceve e decodifica i pacchetti in arrivo da PC
void parser_mess(unsigned char buffer)
{
sensor_msgs::Imu msg;
geometry_msgs::Pose msg_anchor;
//implementazione della macchina a stati
switch(state_msg){
case HEADER_1:
if(buffer == HEADER_A)
{
state_msg=PAYLOAD_1_1;
index_range = 0;
}
if(buffer == HEADER_B)
{
state_msg=PAYLOAD_1_2;
index_range = 0;
}
if(buffer == HEADER_C)
{
state_msg=PAYLOAD_1_3;
index_range = 0;
}
if(buffer == HEADER_D)
{
state_msg=PAYLOAD_1_4;
index_range = 0;
}
if(buffer == HEADER_E)
{
state_msg=PAYLOAD_1_5;
index_range = 0;
}
if(buffer == HEADER_F)
{
state_msg=PAYLOAD_1_6;
index_range = 0;
}
if(buffer == HEADER_G)
{
state_msg=PAYLOAD_1_7;
index_range = 0;
}
if(buffer == HEADER_H)
{
state_msg=PAYLOAD_1_8;
index_range = 0;
}
//calibrazione
if(buffer == HEADER_CALIB)
{
state_msg=PAYLOAD_1_C;
index_range = 0;
}
break;
//////////////////////////////////////////////////////////////////////////////////////////////////
case PAYLOAD_1_1:
switch(buffer)
{
case END_PACKET:
range_recv[index_range] = decode_payload();
//cout << range_recv[index_range] << endl;
msg.angular_velocity.x = range_recv[0];
msg.angular_velocity.y = range_recv[1];
msg.angular_velocity.z = range_recv[2];
msg.linear_acceleration.x = range_recv[3];
range1_pub.publish(msg);
new_packet ++;
state_msg=HEADER_1;
index_range = 0;
break;
case HEADER_A:
for(int i = 0; i< coda_recv_seriale.size(); i++)
coda_recv_seriale.pop();
index_range = 0;
break;
case SEPARATORE:
//ho ricevuto un range, lo devo codificare
range_recv[index_range] = decode_payload();
//cout << range_recv[index_range] << endl;
index_range ++;
break;
default:
coda_recv_seriale.push(buffer);
break;
}
break;
//////////////////////////////////////////////////////////////////////////////////////////////////
case PAYLOAD_1_2:
switch(buffer)
{
case END_PACKET:
range_recv[index_range] = decode_payload();
//cout << range_recv[index_range] << endl;
msg.angular_velocity.x = range_recv[0];
msg.angular_velocity.y = range_recv[1];
msg.angular_velocity.z = range_recv[2];
msg.linear_acceleration.x = range_recv[3];
range2_pub.publish(msg);
new_packet ++;
state_msg=HEADER_1;
index_range = 0;
break;
case HEADER_B:
for(int i = 0; i< coda_recv_seriale.size(); i++)
coda_recv_seriale.pop();
index_range = 0;
break;
case SEPARATORE:
//ho ricevuto un range, lo devo codificare
range_recv[index_range] = decode_payload();
//cout << range_recv[index_range] << endl;
index_range ++;
break;
default:
coda_recv_seriale.push(buffer);
break;
}
break;
//////////////////////////////////////////////////////////////////////////////////////////////////
case PAYLOAD_1_3:
switch(buffer)
{
case END_PACKET:
range_recv[index_range] = decode_payload();
//cout << range_recv[index_range] << endl;
msg.angular_velocity.x = range_recv[0];
msg.angular_velocity.y = range_recv[1];
msg.angular_velocity.z = range_recv[2];
msg.linear_acceleration.x = range_recv[3];
range3_pub.publish(msg);
new_packet ++;
state_msg=HEADER_1;
index_range = 0;
break;
case HEADER_C:
for(int i = 0; i< coda_recv_seriale.size(); i++)
coda_recv_seriale.pop();
index_range = 0;
break;
case SEPARATORE:
//ho ricevuto un range, lo devo codificare
range_recv[index_range] = decode_payload();
//cout << range_recv[index_range] << endl;
index_range ++;
break;
default:
coda_recv_seriale.push(buffer);
break;
}
break;
//////////////////////////////////////////////////////////////////////////////////////////////////
case PAYLOAD_1_4:
switch(buffer)
{
case END_PACKET:
range_recv[index_range] = decode_payload();
//cout << range_recv[index_range] << endl;
msg.angular_velocity.x = range_recv[0];
msg.angular_velocity.y = range_recv[1];
msg.angular_velocity.z = range_recv[2];
msg.linear_acceleration.x = range_recv[3];
range4_pub.publish(msg);
new_packet ++;
state_msg=HEADER_1;
index_range = 0;
break;
case HEADER_D:
for(int i = 0; i< coda_recv_seriale.size(); i++)
coda_recv_seriale.pop();
index_range = 0;
break;
case SEPARATORE:
//ho ricevuto un range, lo devo codificare
range_recv[index_range] = decode_payload();
//cout << range_recv[index_range] << endl;
index_range ++;
break;
default:
coda_recv_seriale.push(buffer);
break;
}
break;
//////////////////////////////////////////////////////////////////////////////////////////////////
case PAYLOAD_1_5:
switch(buffer)
{
case END_PACKET:
range_recv[index_range] = decode_payload();
//cout << range_recv[index_range] << endl;
msg.angular_velocity.x = range_recv[0];
msg.angular_velocity.y = range_recv[1];
msg.angular_velocity.z = range_recv[2];
msg.linear_acceleration.x = range_recv[3];
range5_pub.publish(msg);
new_packet ++;
state_msg=HEADER_1;
index_range = 0;
break;
case HEADER_E:
for(int i = 0; i< coda_recv_seriale.size(); i++)
coda_recv_seriale.pop();
index_range = 0;
break;
case SEPARATORE:
//ho ricevuto un range, lo devo codificare
range_recv[index_range] = decode_payload();
//cout << range_recv[index_range] << endl;
index_range ++;
break;
default:
coda_recv_seriale.push(buffer);
break;
}
break;
//////////////////////////////////////////////////////////////////////////////////////////////////
case PAYLOAD_1_6:
switch(buffer)
{
case END_PACKET:
range_recv[index_range] = decode_payload();
//cout << range_recv[index_range] << endl;
msg.angular_velocity.x = range_recv[0];
msg.angular_velocity.y = range_recv[1];
msg.angular_velocity.z = range_recv[2];
msg.linear_acceleration.x = range_recv[3];
range6_pub.publish(msg);
new_packet ++;
state_msg=HEADER_1;
index_range = 0;
break;
case HEADER_F:
for(int i = 0; i< coda_recv_seriale.size(); i++)
coda_recv_seriale.pop();
index_range = 0;
break;
case SEPARATORE:
//ho ricevuto un range, lo devo codificare
range_recv[index_range] = decode_payload();
//cout << range_recv[index_range] << endl;
index_range ++;
break;
default:
coda_recv_seriale.push(buffer);
break;
}
break;
//////////////////////////////////////////////////////////////////////////////////////////////////
case PAYLOAD_1_7:
switch(buffer)
{
case END_PACKET:
range_recv[index_range] = decode_payload();
//cout << range_recv[index_range] << endl;
msg.angular_velocity.x = range_recv[0];
msg.angular_velocity.y = range_recv[1];
msg.angular_velocity.z = range_recv[2];
msg.linear_acceleration.x = range_recv[3];
range7_pub.publish(msg);
new_packet ++;
state_msg=HEADER_1;
index_range = 0;
break;
case HEADER_G:
for(int i = 0; i< coda_recv_seriale.size(); i++)
coda_recv_seriale.pop();
index_range = 0;
break;
case SEPARATORE:
//ho ricevuto un range, lo devo codificare
range_recv[index_range] = decode_payload();
//cout << range_recv[index_range] << endl;
index_range ++;
break;
default:
coda_recv_seriale.push(buffer);
break;
}
break;
//////////////////////////////////////////////////////////////////////////////////////////////////
case PAYLOAD_1_8:
switch(buffer)
{
case END_PACKET:
range_recv[index_range] = decode_payload();
//cout << range_recv[index_range] << endl;
msg.angular_velocity.x = range_recv[0];
msg.angular_velocity.y = range_recv[1];
msg.angular_velocity.z = range_recv[2];
msg.linear_acceleration.x = range_recv[3];
range8_pub.publish(msg);
new_packet ++;
state_msg=HEADER_1;
index_range = 0;
break;
case HEADER_H:
for(int i = 0; i< coda_recv_seriale.size(); i++)
coda_recv_seriale.pop();
index_range = 0;
break;
case SEPARATORE:
//ho ricevuto un range, lo devo codificare
range_recv[index_range] = decode_payload();
//cout << range_recv[index_range] << endl;
index_range ++;
break;
default:
coda_recv_seriale.push(buffer);
break;
}
break;
//////////////////////////////////////////CALIBRATION//////////////////////
case PAYLOAD_1_C:
switch(buffer)
{
case END_PACKET:
anchor_range[index_range] = decode_payload();
//cout << anchor_range[index_range] << endl;
cout << "Autocalibration response" << endl;
msg_anchor.position.x = anchor_range[0];
msg_anchor.position.y = anchor_range[1];
msg_anchor.position.z = anchor_range[2];
msg_anchor.orientation.x = anchor_range[3];
msg_anchor.orientation.y = anchor_range[4];
msg_anchor.orientation.z = anchor_range[5];
anchor_range_pub.publish(msg_anchor);
//new_packet ++;
state_msg=HEADER_1;
index_range = 0;
break;
case SEPARATORE:
//ho ricevuto un range, lo devo codificare
anchor_range[index_range] = decode_payload();
//cout << anchor_range[index_range] << endl;
index_range ++;
break;
default:
coda_recv_seriale.push(buffer);
break;
}
break;
}
}
