/* invoked once for every Server in ServerList */
static bool decode_serverlist(pb_istream_t *stream, const pb_field_t *field,
void **arg) {
decode_serverlist_arg *dec_arg = *arg;
if (dec_arg->first_pass) { /* count how many server do we have */
grpc_grpclb_server server;
if (!pb_decode(stream, grpc_lb_v1_Server_fields, &server)) {
gpr_log(GPR_ERROR, "nanopb error: %s", PB_GET_ERROR(stream));
return false;
}
dec_arg->num_servers++;
} else { /* second pass. Actually decode. */
grpc_grpclb_server *server = gpr_zalloc(sizeof(grpc_grpclb_server));
GPR_ASSERT(dec_arg->num_servers > 0);
if (dec_arg->decoding_idx == 0) { /* first iteration of second pass */
dec_arg->servers =
gpr_malloc(sizeof(grpc_grpclb_server *) * dec_arg->num_servers);
}
if (!pb_decode(stream, grpc_lb_v1_Server_fields, server)) {
gpr_log(GPR_ERROR, "nanopb error: %s", PB_GET_ERROR(stream));
return false;
}
dec_arg->servers[dec_arg->decoding_idx++] = server;
}
return true;
}
bool decode_trace_context(google_trace_TraceContext *ctxt, uint8_t *buffer,
const size_t nbytes) {
// Create a stream that reads nbytes from the buffer.
pb_istream_t stream = pb_istream_from_buffer(buffer, nbytes);
// decode message
bool status = pb_decode(&stream, google_trace_TraceContext_fields, ctxt);
if (!status) {
gpr_log(GPR_DEBUG, "TraceContext decoding failed: %s",
PB_GET_ERROR(&stream));
return false;
}
// check fields
if (!ctxt->has_trace_id) {
gpr_log(GPR_DEBUG, "Invalid TraceContext: missing trace_id");
return false;
}
if (!ctxt->has_span_id) {
gpr_log(GPR_DEBUG, "Invalid TraceContext: missing span_id");
return false;
}
return true;
}
int main()
{
uint8_t buffer[Person_size];
pb_istream_t stream;
size_t count;
/* Read the data into buffer */
SET_BINARY_MODE(stdin);
count = fread(buffer, 1, sizeof(buffer), stdin);
if (!feof(stdin))
{
printf("Message does not fit in buffer\n");
return 1;
}
/* Construct a pb_istream_t for reading from the buffer */
stream = pb_istream_from_buffer(buffer, count);
/* Decode and print out the stuff */
if (!print_person(&stream))
{
printf("Parsing failed: %s\n", PB_GET_ERROR(&stream));
return 1;
} else {
return 0;
}
}
/**
* encode the gateway message and write the encoded buf to GATEWAY_MSG_PATH
* @param ssmsg
*/
void gwmsg_write(SSMsg *ssmsg)
{
uint8_t buffer[1024];
int msg_len;
FILE *fd;
int status;
pb_ostream_t stream = pb_ostream_from_buffer(buffer, sizeof(buffer));
status=pb_encode(&stream, SSMsg_fields, ssmsg);
msg_len=stream.bytes_written;
if (!status)
{
log_printf(LOG_ERROR,"GateWay Msg Encoding failed: %s\n", PB_GET_ERROR(&stream));
return ;
}
fd=fopen(GATEWAY_MSG_PATH,"wb");
if(fd==NULL)
{
log_printf(LOG_ERROR,"Can not open %s error!\n",GATEWAY_MSG_PATH);
}
fwrite(buffer,msg_len,1,fd);
fclose(fd);
}
int main(int argc, char **argv)
{
uint8_t buffer[1024];
size_t count;
pb_istream_t stream;
/* Whether to expect the optional values or the default values. */
int mode = (argc > 1) ? atoi(argv[1]) : 0;
/* Read the data into buffer */
SET_BINARY_MODE(stdin);
count = fread(buffer, 1, sizeof(buffer), stdin);
/* Construct a pb_istream_t for reading from the buffer */
stream = pb_istream_from_buffer(buffer, count);
/* Decode and print out the stuff */
if (!check_alltypes(&stream, mode))
{
printf("Parsing failed: %s\n", PB_GET_ERROR(&stream));
return 1;
} else {
return 0;
}
}
void MotorController::sendRequest()
{
/* This is the buffer where we will store the request message. */
uint8_t requestbuffer[256];
/* allocate space for the request message to the server */
RequestMessage request = RequestMessage_init_zero;
/* Create a stream that will write to our buffer. */
pb_ostream_t ostream = pb_ostream_from_buffer(requestbuffer, sizeof(requestbuffer));
/* indicate that speed fields will contain values */
request.has_speed_l = true;
request.has_speed_r = true;
/* fill in the message fields */
request.speed_l = static_cast<float>(current_motor_command_.left_velocity);
request.speed_r = static_cast<float>(current_motor_command_.right_velocity);
/* encode the protobuffer */
bool status = pb_encode(&ostream, RequestMessage_fields, &request);
size_t message_length = ostream.bytes_written;
/* check for any errors.. */
if (!status)
{
ROS_ERROR_STREAM("Encoding failed: " << PB_GET_ERROR(&ostream));
ros::shutdown();
}
/* Send the message strapped to a pigeon's leg! */
(*sock_).sendMessage(reinterpret_cast<char*>(requestbuffer), message_length);
}
// encode message struct in msg according to 'fields', and send off via rb
// return 0 on success or < 0 on failure; see rtmsg_errors_t
//
static int npb_send_msg(const void *msg, const pb_field_t *fields,
ringbuffer_t *rb, const hal_funct_args_t *fa)
{
void *buffer;
int retval;
size_t size;
// determine size
pb_ostream_t sstream = PB_OSTREAM_SIZING;
if (!pb_encode(&sstream, fields, msg)) {
rtapi_print_msg(RTAPI_MSG_ERR,
"%s: sizing pb_encode(): %s written=%zu\n",
fa_funct_name(fa), PB_GET_ERROR(&sstream), sstream.bytes_written);
return NBP_SIZE_FAIL;
}
size = sstream.bytes_written;
// preallocate memory in ringbuffer
if ((retval = record_write_begin(rb, (void **)&buffer, size)) != 0) {
rtapi_print_msg(RTAPI_MSG_ERR,
"%s: record_write_begin(%zu) failed: %d\n",
fa_funct_name(fa), size, retval);
return RB_RESERVE_FAIL;
}
// zero-copy encode directly into ringbuffer
pb_ostream_t rstream = pb_ostream_from_buffer(buffer, size);
if (!pb_encode(&rstream, fields, msg)) {
rtapi_print_msg(RTAPI_MSG_ERR,
"%s: pb_encode failed: %s, size=%zu written=%zu\n",
fa_funct_name(fa),
PB_GET_ERROR(&rstream),
size,
rstream.bytes_written);
return NBP_ENCODE_FAIL;
}
// send it off
if ((retval = record_write_end(rb, buffer, rstream.bytes_written))) {
rtapi_print_msg(RTAPI_MSG_ERR,
"%s: record_write_end(%zu) failed: %d\n",
fa_funct_name(fa), size, retval);
return RB_WRITE_FAIL;
}
return 0;
}
int main()
{
pb_istream_t stream = {&callback, stdin, 10000};
if (!print_container(&stream))
{
printf("Parsing failed: %s\n", PB_GET_ERROR(&stream));
return 1;
} else {
return 0;
}
}
bool print_container(pb_istream_t *stream)
{
uint64_t type, msgtype;
uint32_t length, tag;
pb_wire_type_t wiretype;
bool eof;
// decode the type tag
if (!pb_decode_tag(stream, &wiretype, &tag, &eof)) {
printf("Parsing tag#1 failed: %s\n", PB_GET_ERROR(stream));
}
if (!pb_decode_varint(stream, &type)) {
printf("Parsing required type field#1 failed: %s\n", PB_GET_ERROR(stream));
}
if (!pb_decode_tag(stream, &wiretype, &tag, &eof)) {
printf("Parsing tag#2 failed: %s\n", PB_GET_ERROR(stream));
}
assert(tag == 1); // tag number of length field
assert(wiretype == 5); // encoding of length field = fixed32
if (!pb_decode_fixed32(stream, &length)) {
printf("Parsing field#1 failed: %s\n", PB_GET_ERROR(stream));
}
printf("wiretype=%d tag=%d fixed32=%d (total message length)\n",
wiretype, tag, length);
if (!pb_decode_tag(stream, &wiretype, &tag, &eof)) {
printf("Parsing tag#2 failed: %s\n", PB_GET_ERROR(stream));
}
assert(tag == 2); // tag number of type field
assert(wiretype == 0); // encoding of length field = varint
if (!pb_decode_varint(stream, &msgtype)) {
printf("Parsing field#2 failed: %s\n", PB_GET_ERROR(stream));
}
printf("wiretype=%d tag=%d varint=%llu (msgtype)\n", wiretype, tag, msgtype);
// the actual message follows.
// It is a submessage encoded in length-delimited format
if (!pb_decode_tag(stream, &wiretype, &tag, &eof)) {
printf("Parsing tag#3 failed: %s\n", PB_GET_ERROR(stream));
}
printf("wiretype=%d tag=%d (submessage type)\n", wiretype, tag);
assert(wiretype == 2); // length-delimited format
printf("submessage: %s NML; %s Motion\n",
is_NML_container(tag) ? "is" : "not",
is_Motion_container(tag) ? "is" : "not");
// decoding the submessage left as exercise
return true;
}
int main()
{
pb_istream_t stream = {&callback, NULL, SIZE_MAX};
stream.state = stdin;
SET_BINARY_MODE(stdin);
if (!print_person(&stream))
{
printf("Parsing failed: %s\n", PB_GET_ERROR(&stream));
return 1;
} else {
return 0;
}
}
grpc_grpclb_serverlist *grpc_grpclb_response_parse_serverlist(
grpc_slice encoded_grpc_grpclb_response) {
bool status;
decode_serverlist_arg arg;
pb_istream_t stream =
pb_istream_from_buffer(GRPC_SLICE_START_PTR(encoded_grpc_grpclb_response),
GRPC_SLICE_LENGTH(encoded_grpc_grpclb_response));
pb_istream_t stream_at_start = stream;
grpc_grpclb_response res;
memset(&res, 0, sizeof(grpc_grpclb_response));
memset(&arg, 0, sizeof(decode_serverlist_arg));
res.server_list.servers.funcs.decode = decode_serverlist;
res.server_list.servers.arg = &arg;
arg.first_pass = true;
status = pb_decode(&stream, grpc_lb_v1_LoadBalanceResponse_fields, &res);
if (!status) {
gpr_log(GPR_ERROR, "nanopb error: %s", PB_GET_ERROR(&stream));
return NULL;
}
arg.first_pass = false;
status =
pb_decode(&stream_at_start, grpc_lb_v1_LoadBalanceResponse_fields, &res);
if (!status) {
gpr_log(GPR_ERROR, "nanopb error: %s", PB_GET_ERROR(&stream));
return NULL;
}
grpc_grpclb_serverlist *sl = gpr_zalloc(sizeof(grpc_grpclb_serverlist));
sl->num_servers = arg.num_servers;
sl->servers = arg.servers;
if (res.server_list.has_expiration_interval) {
sl->expiration_interval = res.server_list.expiration_interval;
}
return sl;
}
// This function assumes the TraceContext is valid.
size_t encode_trace_context(google_trace_TraceContext *ctxt, uint8_t *buffer,
const size_t buf_size) {
// Create a stream that will write to our buffer.
pb_ostream_t stream = pb_ostream_from_buffer(buffer, buf_size);
// encode message
bool status = pb_encode(&stream, google_trace_TraceContext_fields, ctxt);
if (!status) {
gpr_log(GPR_DEBUG, "TraceContext encoding failed: %s",
PB_GET_ERROR(&stream));
return 0;
}
return stream.bytes_written;
}
/**
* Message SSMsg decode function
* @param buffer
* @param buf_len
* @param ssmsg
* @return
*/
int ss_pb_decode(uint8_t *buffer,int buf_len,SSMsg *ssmsg)
{
bool status;
pb_istream_t stream = pb_istream_from_buffer(buffer, buf_len);
//ssmsg->devlist.funcs.decode=&ss_decode_devlist;
log_printf(LOG_VERBOSE,"SSMsg Decoding! \n");
status=pb_decode(&stream, SSMsg_fields, ssmsg);
if (!status)
{
log_printf(LOG_ERROR,"Decoding failed: %s\n", PB_GET_ERROR(&stream));
return -1;
}
return 1;
}
grpc_grpclb_initial_response *grpc_grpclb_initial_response_parse(
grpc_slice encoded_grpc_grpclb_response) {
pb_istream_t stream =
pb_istream_from_buffer(GRPC_SLICE_START_PTR(encoded_grpc_grpclb_response),
GRPC_SLICE_LENGTH(encoded_grpc_grpclb_response));
grpc_grpclb_response res;
memset(&res, 0, sizeof(grpc_grpclb_response));
if (!pb_decode(&stream, grpc_lb_v1_LoadBalanceResponse_fields, &res)) {
gpr_log(GPR_ERROR, "nanopb error: %s", PB_GET_ERROR(&stream));
return NULL;
}
grpc_grpclb_initial_response *initial_res =
gpr_malloc(sizeof(grpc_grpclb_initial_response));
memcpy(initial_res, &res.initial_response,
sizeof(grpc_grpclb_initial_response));
return initial_res;
}
bool process_incoming_packet(pb_istream_t* isrt)
{
bool status = false;
const pb_field_t *type = decode_unionmessage_type(isrt, MessagePackage_fields);
if (SimpleCommand_fields == type)
{
status = decode_unionmessage_contents(isrt, SimpleCommand_fields, &command);
if (status)
process_command();
}
else if (SimpleAnswer_fields == type)
{
status = decode_unionmessage_contents(isrt, SimpleAnswer_fields, &answer);
if (status)
process_answer();
}
else if (DebugPrint_fields == type)
{
status = decode_unionmessage_contents(isrt, DebugPrint_fields, &debug_print_message);
if (status)
process_debug_print_package();
}
else if (PositionNotify_fields == type)
{
status = decode_unionmessage_contents(isrt, PositionNotify_fields, &pos_notify);
if (status)
process_position_notify();
}
else
{
// Serial.println("Unknown package");
// ERROR_BLINK(9);
}
if (!status)
{
DEBUG_PRINTLN2("process_incoming_packet error ", PB_GET_ERROR(isrt));
}
return status;
}
static int decode_msg(pbring_inst_t *p, pb_istream_t *stream, const hal_funct_args_t *fa)
{
if (!pb_decode(stream, pb_Container_fields, &p->rx)) {
*(p->decodefail) += 1;
rtapi_print_msg(RTAPI_MSG_ERR, "%s: pb_decode(Container) failed: '%s'\n",
fa_funct_name(fa), PB_GET_ERROR(stream));
return -1;
}
if (p->rx.has_motcmd) {
rtapi_print_msg(RTAPI_MSG_ERR, "Container.motcmd command=%d num=%d\n",
p->rx.motcmd.command,p->rx.motcmd.commandNum);
if (p->rx.motcmd.has_pos) {
char buf[200];
rtapi_format_pose(buf, sizeof(buf), &p->rx.motcmd.pos);
rtapi_print_msg(RTAPI_MSG_ERR, "motcmd: %s\n", buf);
}
}
return 0;
}
/* Test the 'AnonymousOneOfMessage' */
int test_oneof_1(pb_istream_t *stream, int option)
{
AnonymousOneOfMessage msg;
int status = 0;
/* To better catch initialization errors */
memset(&msg, 0xAA, sizeof(msg));
if (!pb_decode(stream, AnonymousOneOfMessage_fields, &msg))
{
printf("Decoding failed: %s\n", PB_GET_ERROR(stream));
return 1;
}
/* Check that the basic fields work normally */
TEST(msg.prefix == 123);
TEST(msg.suffix == 321);
/* Check that we got the right oneof according to command line */
if (option == 1)
{
TEST(msg.which_values == AnonymousOneOfMessage_first_tag);
TEST(msg.first == 999);
}
else if (option == 2)
{
TEST(msg.which_values == AnonymousOneOfMessage_second_tag);
TEST(strcmp(msg.second, "abcd") == 0);
}
else if (option == 3)
{
TEST(msg.which_values == AnonymousOneOfMessage_third_tag);
TEST(msg.third.array[0] == 1);
TEST(msg.third.array[1] == 2);
TEST(msg.third.array[2] == 3);
TEST(msg.third.array[3] == 4);
TEST(msg.third.array[4] == 5);
}
return status;
}
int main(int argc, char *argv[])
{
uint8_t buffer[256];
pb_ostream_t stream = pb_ostream_from_buffer(buffer, sizeof(buffer));
Dictionary dict = Dictionary_init_zero;
if (argc <= 1)
{
fprintf(stderr, "Usage: %s \"{'foobar': 1234, ...}\"\n", argv[0]);
return 1;
}
dict.dictItem.funcs.encode = encode_dictionary;
dict.dictItem.arg = argv[1];
if (!pb_encode(&stream, Dictionary_fields, &dict))
{
fprintf(stderr, "Encoding error: %s\n", PB_GET_ERROR(&stream));
return 1;
}
fwrite(buffer, 1, stream.bytes_written, stdout);
return 0;
}
void MotorController::recieveResponse()
{
/* Read response from the server */
size_t n; // n is the response from socket: 0 means connection closed, otherwise n = num bytes read
uint8_t buffer[256]; // buffer to read response into
memset(buffer, 0, sizeof(buffer));
/* read from the buffer */
n = (*sock_).readMessage(buffer); // blocks until data is read
if (n == 0)
{
ROS_ERROR_STREAM("Connection closed by server");
ros::shutdown();
}
/* Allocate space for the decoded message. */
ResponseMessage response = ResponseMessage_init_zero;
/* Create a stream that reads from the buffer. */
pb_istream_t istream = pb_istream_from_buffer(buffer, n);
/* decode the message. */
bool status = pb_decode(&istream, ResponseMessage_fields, &response);
/* check for any errors.. */
if (!status)
{
ROS_ERROR_STREAM("Decoding Failed: " << PB_GET_ERROR(&istream));
ros::shutdown();
}
publishResponse(response);
ROS_INFO_STREAM_THROTTLE(log_period_, "Rate: " << 1 / response.dt_sec << "hz.");
}
int main(int argc, char **argv)
{
int mode = (argc > 1) ? atoi(argv[1]) : 0;
/* Values for use from callbacks through pointers. */
uint32_t req_fixed32 = 1008;
int32_t req_sfixed32 = -1009;
float req_float = 1010.0f;
uint64_t req_fixed64 = 1011;
int64_t req_sfixed64 = -1012;
double req_double = 1013.0;
SubMessage req_submsg = {"1016", 1016};
uint32_t rep_fixed32 = 2008;
int32_t rep_sfixed32 = -2009;
float rep_float = 2010.0f;
uint64_t rep_fixed64 = 2011;
int64_t rep_sfixed64 = -2012;
double rep_double = 2013.0;
SubMessage rep_submsg = {"2016", 2016, true, 2016};
uint32_t opt_fixed32 = 3048;
int32_t opt_sfixed32 = 3049;
float opt_float = 3050.0f;
uint64_t opt_fixed64 = 3051;
int64_t opt_sfixed64 = 3052;
double opt_double = 3053.0f;
SubMessage opt_submsg = {"3056", 3056};
SubMessage oneof_msg1 = {"4059", 4059};
/* Bind callbacks for required fields */
AllTypes alltypes = {{{0}}};
alltypes.req_int32.funcs.encode = &write_varint;
alltypes.req_int32.arg = (void*)-1001;
alltypes.req_int64.funcs.encode = &write_varint;
alltypes.req_int64.arg = (void*)-1002;
alltypes.req_uint32.funcs.encode = &write_varint;
alltypes.req_uint32.arg = (void*)1003;
alltypes.req_uint32.funcs.encode = &write_varint;
alltypes.req_uint32.arg = (void*)1003;
alltypes.req_uint64.funcs.encode = &write_varint;
alltypes.req_uint64.arg = (void*)1004;
alltypes.req_sint32.funcs.encode = &write_svarint;
alltypes.req_sint32.arg = (void*)-1005;
alltypes.req_sint64.funcs.encode = &write_svarint;
alltypes.req_sint64.arg = (void*)-1006;
alltypes.req_bool.funcs.encode = &write_varint;
alltypes.req_bool.arg = (void*)true;
alltypes.req_fixed32.funcs.encode = &write_fixed32;
alltypes.req_fixed32.arg = &req_fixed32;
alltypes.req_sfixed32.funcs.encode = &write_fixed32;
alltypes.req_sfixed32.arg = &req_sfixed32;
alltypes.req_float.funcs.encode = &write_fixed32;
alltypes.req_float.arg = &req_float;
alltypes.req_fixed64.funcs.encode = &write_fixed64;
alltypes.req_fixed64.arg = &req_fixed64;
alltypes.req_sfixed64.funcs.encode = &write_fixed64;
alltypes.req_sfixed64.arg = &req_sfixed64;
alltypes.req_double.funcs.encode = &write_fixed64;
alltypes.req_double.arg = &req_double;
alltypes.req_string.funcs.encode = &write_string;
alltypes.req_string.arg = "1014";
alltypes.req_bytes.funcs.encode = &write_string;
alltypes.req_bytes.arg = "1015";
alltypes.req_submsg.funcs.encode = &write_submsg;
alltypes.req_submsg.arg = &req_submsg;
alltypes.req_enum.funcs.encode = &write_varint;
alltypes.req_enum.arg = (void*)MyEnum_Truth;
alltypes.req_emptymsg.funcs.encode = &write_emptymsg;
/* Bind callbacks for repeated fields */
alltypes.rep_int32.funcs.encode = &write_repeated_varint;
alltypes.rep_int32.arg = (void*)-2001;
alltypes.rep_int64.funcs.encode = &write_repeated_varint;
alltypes.rep_int64.arg = (void*)-2002;
alltypes.rep_uint32.funcs.encode = &write_repeated_varint;
alltypes.rep_uint32.arg = (void*)2003;
alltypes.rep_uint64.funcs.encode = &write_repeated_varint;
alltypes.rep_uint64.arg = (void*)2004;
alltypes.rep_sint32.funcs.encode = &write_repeated_svarint;
alltypes.rep_sint32.arg = (void*)-2005;
alltypes.rep_sint64.funcs.encode = &write_repeated_svarint;
alltypes.rep_sint64.arg = (void*)-2006;
alltypes.rep_bool.funcs.encode = &write_repeated_varint;
alltypes.rep_bool.arg = (void*)true;
alltypes.rep_fixed32.funcs.encode = &write_repeated_fixed32;
alltypes.rep_fixed32.arg = &rep_fixed32;
alltypes.rep_sfixed32.funcs.encode = &write_repeated_fixed32;
alltypes.rep_sfixed32.arg = &rep_sfixed32;
alltypes.rep_float.funcs.encode = &write_repeated_fixed32;
alltypes.rep_float.arg = &rep_float;
alltypes.rep_fixed64.funcs.encode = &write_repeated_fixed64;
alltypes.rep_fixed64.arg = &rep_fixed64;
alltypes.rep_sfixed64.funcs.encode = &write_repeated_fixed64;
alltypes.rep_sfixed64.arg = &rep_sfixed64;
alltypes.rep_double.funcs.encode = &write_repeated_fixed64;
alltypes.rep_double.arg = &rep_double;
alltypes.rep_string.funcs.encode = &write_repeated_string;
alltypes.rep_string.arg = "2014";
alltypes.rep_bytes.funcs.encode = &write_repeated_string;
alltypes.rep_bytes.arg = "2015";
alltypes.rep_submsg.funcs.encode = &write_repeated_submsg;
alltypes.rep_submsg.arg = &rep_submsg;
alltypes.rep_enum.funcs.encode = &write_repeated_varint;
alltypes.rep_enum.arg = (void*)MyEnum_Truth;
alltypes.rep_emptymsg.funcs.encode = &write_repeated_emptymsg;
alltypes.req_limits.funcs.encode = &write_limits;
/* Bind callbacks for optional fields */
if (mode != 0)
{
alltypes.opt_int32.funcs.encode = &write_varint;
alltypes.opt_int32.arg = (void*)3041;
alltypes.opt_int64.funcs.encode = &write_varint;
alltypes.opt_int64.arg = (void*)3042;
alltypes.opt_uint32.funcs.encode = &write_varint;
alltypes.opt_uint32.arg = (void*)3043;
alltypes.opt_uint64.funcs.encode = &write_varint;
alltypes.opt_uint64.arg = (void*)3044;
alltypes.opt_sint32.funcs.encode = &write_svarint;
alltypes.opt_sint32.arg = (void*)3045;
alltypes.opt_sint64.funcs.encode = &write_svarint;
alltypes.opt_sint64.arg = (void*)3046;
alltypes.opt_bool.funcs.encode = &write_varint;
alltypes.opt_bool.arg = (void*)true;
alltypes.opt_fixed32.funcs.encode = &write_fixed32;
alltypes.opt_fixed32.arg = &opt_fixed32;
alltypes.opt_sfixed32.funcs.encode = &write_fixed32;
alltypes.opt_sfixed32.arg = &opt_sfixed32;
alltypes.opt_float.funcs.encode = &write_fixed32;
alltypes.opt_float.arg = &opt_float;
alltypes.opt_fixed64.funcs.encode = &write_fixed64;
alltypes.opt_fixed64.arg = &opt_fixed64;
alltypes.opt_sfixed64.funcs.encode = &write_fixed64;
alltypes.opt_sfixed64.arg = &opt_sfixed64;
alltypes.opt_double.funcs.encode = &write_fixed64;
alltypes.opt_double.arg = &opt_double;
alltypes.opt_string.funcs.encode = &write_string;
alltypes.opt_string.arg = "3054";
alltypes.opt_bytes.funcs.encode = &write_string;
alltypes.opt_bytes.arg = "3055";
alltypes.opt_submsg.funcs.encode = &write_submsg;
alltypes.opt_submsg.arg = &opt_submsg;
alltypes.opt_enum.funcs.encode = &write_varint;
alltypes.opt_enum.arg = (void*)MyEnum_Truth;
alltypes.opt_emptymsg.funcs.encode = &write_emptymsg;
alltypes.oneof_msg1.funcs.encode = &write_submsg;
alltypes.oneof_msg1.arg = &oneof_msg1;
}
alltypes.end.funcs.encode = &write_varint;
alltypes.end.arg = (void*)1099;
{
uint8_t buffer[2048];
pb_ostream_t stream = pb_ostream_from_buffer(buffer, sizeof(buffer));
/* Now encode it and check if we succeeded. */
if (pb_encode(&stream, AllTypes_fields, &alltypes))
{
SET_BINARY_MODE(stdout);
fwrite(buffer, 1, stream.bytes_written, stdout);
return 0; /* Success */
}
else
{
fprintf(stderr, "Encoding failed: %s\n", PB_GET_ERROR(&stream));
return 1; /* Failure */
}
}
}
// thread function, per-instance
// interpolates all joints of this instance
static int update(void *arg, const hal_funct_args_t *fa)
{
struct inst_data *ip = (struct inst_data *) arg;
double period = ((double) fa_period(fa)) * 1e-9;
int i;
if (segment_completed(ip, period)) {
// check for a new JointTrajectoryPoint
void *data;
ringsize_t size;
if (record_read(&ip->traj, (const void**)&data, &size) == 0) {
// protobuf-decode it
pb_istream_t stream = pb_istream_from_buffer(data, size);
pb_JointTrajectoryPoint rx = pb_JointTrajectoryPoint_init_zero;
if (!pb_decode(&stream, pb_JointTrajectoryPoint_fields, &rx)) {
rtapi_print_msg(RTAPI_MSG_ERR, "%s: pb_decode(JointTrajectoryPoint) failed: '%s'",
compname, PB_GET_ERROR(&stream));
} else {
// decode ok - start a new segment
double duration = *(ip->duration) = rx.time_from_start - ip->time_from_start;
// the very first point in the ringbuffer is not a segment.
// therefore we need to "jump" to these initial settings for the
// interpolator to calculate the correct path.
// for example, a path can start at position, velocity and acceleration
// who are non-zero. In a typical ROS message the first point has a
// duration of "0.0"
if (duration == 0.0) {
// set the start positions
// or try out to drop this point later on
for (i = 0; i < ip->count; i++) {
struct joint *jp = &ip->joints[i];
*(jp->traj_busy) = true;
*(jp->curr_pos) = *(jp->end_pos) = rx.positions[i];
*(jp->curr_vel) = *(jp->end_vel) = rx.velocities[i];
*(jp->curr_acc) = *(jp->end_acc) = rx.accelerations[i];
jp->coeff[0] = *(jp->end_pos);
jp->coeff[1] = 0.0;
jp->coeff[2] = 0.0;
jp->coeff[3] = 0.0;
jp->coeff[4] = 0.0;
jp->coeff[5] = 0.0;
}
// so when we have read the first point, we need to discard everythin
// else and make sure we will read the second point, as to complete the
// first segment
} else {
generatePowers(*(ip->degree), duration, ip->powers);
ip->time_from_start = rx.time_from_start;
*(ip->progress) = 0.0;
for (i = 0; i < rx.positions_count; i++) {
struct joint *jp = &ip->joints[i];
*(jp->traj_busy) = true;
double pos2 = *(jp->end_pos) = rx.positions[i];
double vel2 = *(jp->end_vel) = rx.velocities[i];
double acc2 = *(jp->end_acc) = rx.accelerations[i];
double pos1 = *(jp->curr_pos);
double vel1 = *(jp->curr_vel);
double acc1 = *(jp->curr_acc);
switch (*(ip->degree)) {
case 1:
jp->coeff[0] = pos1;
jp->coeff[1] = (pos2 - pos1) / duration;
jp->coeff[2] = 0.0;
jp->coeff[3] = 0.0;
jp->coeff[4] = 0.0;
jp->coeff[5] = 0.0;
break;
case 3:
jp->coeff[0] = pos1;
jp->coeff[1] = vel1;
jp->coeff[2] = (-3.0*pos1 + 3.0*pos2 - 2.0*vel1*ip->powers[1] - vel2*ip->powers[1]) / ip->powers[2];
jp->coeff[3] = (2.0*pos1 - 2.0*pos2 + vel1*ip->powers[1] + vel2*ip->powers[1]) / ip->powers[3];
jp->coeff[4] = 0.0;
jp->coeff[5] = 0.0;
break;
case 5:
jp->coeff[0] = pos1;
jp->coeff[1] = vel1;
jp->coeff[2] = 0.5 * acc1;
jp->coeff[3] = (-20.0*pos1 + 20.0*pos2 - 3.0*acc1*ip->powers[2] + acc2*ip->powers[2] -
12.0*vel1*ip->powers[1] - 8.0*vel2*ip->powers[1]) / (2.0*ip->powers[3]);
jp->coeff[4] = (30.0*pos1 - 30.0*pos2 + 3.0*acc1*ip->powers[2] - 2.0*acc2*ip->powers[2] +
16.0*vel1*ip->powers[1] + 14.0*vel2*ip->powers[1]) / (2.0*ip->powers[4]);
jp->coeff[5] = (-12.0*pos1 + 12.0*pos2 - acc1*ip->powers[2] + acc2*ip->powers[2] -
6.0*vel1*ip->powers[1] - 6.0*vel2*ip->powers[1]) / (2.0*ip->powers[5]);
break;
}
}
}
}
record_shift(&ip->traj); // consume record
} else {
// segment completed and no new point in ringbuffer
for (i = 0; i < ip->count; i++) {
struct joint *jp = &ip->joints[i];
*(jp->traj_busy) = false;
jp->coeff[0] = *(jp->end_pos);
jp->coeff[1] = 0.0;
jp->coeff[2] = 0.0;
jp->coeff[3] = 0.0;
jp->coeff[4] = 0.0;
jp->coeff[5] = 0.0;
}
}
}
*(ip->progress) += period;
generatePowers(*(ip->degree), *(ip->progress), ip->pnow);
for (i = 0; i < ip->count; i++) {
struct joint *jp = &ip->joints[i];
interpolate_joint(ip, jp, *(ip->progress), 0);
}
return 0;
}
/* Basic fields, nested submessages, extensions */
static bool test_TestMessage()
{
uint8_t buffer[256];
size_t msgsize;
/* Construct a message with various fields filled in */
{
TestMessage msg = TestMessage_init_zero;
pb_ostream_t stream;
fill_TestMessage(&msg);
stream = pb_ostream_from_buffer(buffer, sizeof(buffer));
if (!pb_encode(&stream, TestMessage_fields, &msg))
{
fprintf(stderr, "Encode failed: %s\n", PB_GET_ERROR(&stream));
return false;
}
msgsize = stream.bytes_written;
}
/* Output encoded message for debug */
SET_BINARY_MODE(stdout);
fwrite(buffer, 1, msgsize, stdout);
/* Decode memory using dynamic allocation */
{
TestMessage msg = TestMessage_init_zero;
pb_istream_t stream;
SubMessage ext2_dest;
msg.extensions = &ext1;
ext1.type = &dynamic_ext;
ext1.dest = NULL;
ext1.next = &ext2;
ext2.type = &static_ext;
ext2.dest = &ext2_dest;
ext2.next = NULL;
stream = pb_istream_from_buffer(buffer, msgsize);
if (!pb_decode(&stream, TestMessage_fields, &msg))
{
fprintf(stderr, "Decode failed: %s\n", PB_GET_ERROR(&stream));
return false;
}
/* Make sure it encodes back to same data */
{
uint8_t buffer2[256];
pb_ostream_t ostream = pb_ostream_from_buffer(buffer2, sizeof(buffer2));
TEST(pb_encode(&ostream, TestMessage_fields, &msg));
TEST(ostream.bytes_written == msgsize);
TEST(memcmp(buffer, buffer2, msgsize) == 0);
}
/* Make sure that malloc counters work */
TEST(get_alloc_count() > 0);
/* Make sure that pb_release releases everything */
pb_release(TestMessage_fields, &msg);
TEST(get_alloc_count() == 0);
/* Check that double-free is a no-op */
pb_release(TestMessage_fields, &msg);
TEST(get_alloc_count() == 0);
}
return true;
}
int main(int argc, char **argv)
{
int mode = (argc > 1) ? atoi(argv[1]) : 0;
/* Values for required fields */
int32_t req_int32 = -1001;
int64_t req_int64 = -1002;
uint32_t req_uint32 = 1003;
uint64_t req_uint64 = 1004;
int32_t req_sint32 = -1005;
int64_t req_sint64 = -1006;
bool req_bool = true;
uint32_t req_fixed32 = 1008;
int32_t req_sfixed32 = -1009;
float req_float = 1010.0f;
uint64_t req_fixed64 = 1011;
int64_t req_sfixed64 = -1012;
double req_double = 1013.0;
char* req_string = "1014";
PB_BYTES_ARRAY_T(4) req_bytes = {4, {'1', '0', '1', '5'}};
static int32_t req_substuff = 1016;
SubMessage req_submsg = {"1016", &req_substuff};
MyEnum req_enum = MyEnum_Truth;
EmptyMessage req_emptymsg = {0};
int32_t end = 1099;
/* Values for repeated fields */
int32_t rep_int32[5] = {0, 0, 0, 0, -2001};
int64_t rep_int64[5] = {0, 0, 0, 0, -2002};
uint32_t rep_uint32[5] = {0, 0, 0, 0, 2003};
uint64_t rep_uint64[5] = {0, 0, 0, 0, 2004};
int32_t rep_sint32[5] = {0, 0, 0, 0, -2005};
int64_t rep_sint64[5] = {0, 0, 0, 0, -2006};
bool rep_bool[5] = {false, false, false, false, true};
uint32_t rep_fixed32[5] = {0, 0, 0, 0, 2008};
int32_t rep_sfixed32[5] = {0, 0, 0, 0, -2009};
float rep_float[5] = {0, 0, 0, 0, 2010.0f};
uint64_t rep_fixed64[5] = {0, 0, 0, 0, 2011};
int64_t rep_sfixed64[5] = {0, 0, 0, 0, -2012};
double rep_double[5] = {0, 0, 0, 0, 2013.0f};
char* rep_string[5] = {"", "", "", "", "2014"};
static PB_BYTES_ARRAY_T(4) rep_bytes_4 = {4, {'2', '0', '1', '5'}};
pb_bytes_array_t *rep_bytes[5]= {NULL, NULL, NULL, NULL, (pb_bytes_array_t*)&rep_bytes_4};
static int32_t rep_sub2zero = 0;
static int32_t rep_substuff2 = 2016;
static uint32_t rep_substuff3 = 2016;
SubMessage rep_submsg[5] = {{"", &rep_sub2zero},
{"", &rep_sub2zero},
{"", &rep_sub2zero},
{"", &rep_sub2zero},
{"2016", &rep_substuff2, &rep_substuff3}};
MyEnum rep_enum[5] = {0, 0, 0, 0, MyEnum_Truth};
EmptyMessage rep_emptymsg[5] = {{0}, {0}, {0}, {0}, {0}};
/* Values for optional fields */
int32_t opt_int32 = 3041;
int64_t opt_int64 = 3042;
uint32_t opt_uint32 = 3043;
uint64_t opt_uint64 = 3044;
int32_t opt_sint32 = 3045;
int64_t opt_sint64 = 3046;
bool opt_bool = true;
uint32_t opt_fixed32 = 3048;
int32_t opt_sfixed32 = 3049;
float opt_float = 3050.0f;
uint64_t opt_fixed64 = 3051;
int64_t opt_sfixed64 = 3052;
double opt_double = 3053.0;
char* opt_string = "3054";
PB_BYTES_ARRAY_T(4) opt_bytes = {4, {'3', '0', '5', '5'}};
static int32_t opt_substuff = 3056;
SubMessage opt_submsg = {"3056", &opt_substuff};
MyEnum opt_enum = MyEnum_Truth;
EmptyMessage opt_emptymsg = {0};
/* Values for the Limits message. */
static int32_t int32_min = INT32_MIN;
static int32_t int32_max = INT32_MAX;
static uint32_t uint32_min = 0;
static uint32_t uint32_max = UINT32_MAX;
static int64_t int64_min = INT64_MIN;
static int64_t int64_max = INT64_MAX;
static uint64_t uint64_min = 0;
static uint64_t uint64_max = UINT64_MAX;
static HugeEnum enum_min = HugeEnum_Negative;
static HugeEnum enum_max = HugeEnum_Positive;
Limits req_limits = {&int32_min, &int32_max,
&uint32_min, &uint32_max,
&int64_min, &int64_max,
&uint64_min, &uint64_max,
&enum_min, &enum_max};
/* Initialize the message struct with pointers to the fields. */
AllTypes alltypes = {0};
alltypes.req_int32 = &req_int32;
alltypes.req_int64 = &req_int64;
alltypes.req_uint32 = &req_uint32;
alltypes.req_uint64 = &req_uint64;
alltypes.req_sint32 = &req_sint32;
alltypes.req_sint64 = &req_sint64;
alltypes.req_bool = &req_bool;
alltypes.req_fixed32 = &req_fixed32;
alltypes.req_sfixed32 = &req_sfixed32;
alltypes.req_float = &req_float;
alltypes.req_fixed64 = &req_fixed64;
alltypes.req_sfixed64 = &req_sfixed64;
alltypes.req_double = &req_double;
alltypes.req_string = req_string;
alltypes.req_bytes = (pb_bytes_array_t*)&req_bytes;
alltypes.req_submsg = &req_submsg;
alltypes.req_enum = &req_enum;
alltypes.req_emptymsg = &req_emptymsg;
alltypes.req_limits = &req_limits;
alltypes.rep_int32_count = 5; alltypes.rep_int32 = rep_int32;
alltypes.rep_int64_count = 5; alltypes.rep_int64 = rep_int64;
alltypes.rep_uint32_count = 5; alltypes.rep_uint32 = rep_uint32;
alltypes.rep_uint64_count = 5; alltypes.rep_uint64 = rep_uint64;
alltypes.rep_sint32_count = 5; alltypes.rep_sint32 = rep_sint32;
alltypes.rep_sint64_count = 5; alltypes.rep_sint64 = rep_sint64;
alltypes.rep_bool_count = 5; alltypes.rep_bool = rep_bool;
alltypes.rep_fixed32_count = 5; alltypes.rep_fixed32 = rep_fixed32;
alltypes.rep_sfixed32_count = 5; alltypes.rep_sfixed32 = rep_sfixed32;
alltypes.rep_float_count = 5; alltypes.rep_float = rep_float;
alltypes.rep_fixed64_count = 5; alltypes.rep_fixed64 = rep_fixed64;
alltypes.rep_sfixed64_count = 5; alltypes.rep_sfixed64 = rep_sfixed64;
alltypes.rep_double_count = 5; alltypes.rep_double = rep_double;
alltypes.rep_string_count = 5; alltypes.rep_string = rep_string;
alltypes.rep_bytes_count = 5; alltypes.rep_bytes = rep_bytes;
alltypes.rep_submsg_count = 5; alltypes.rep_submsg = rep_submsg;
alltypes.rep_enum_count = 5; alltypes.rep_enum = rep_enum;
alltypes.rep_emptymsg_count = 5; alltypes.rep_emptymsg = rep_emptymsg;
if (mode != 0)
{
/* Fill in values for optional fields */
alltypes.opt_int32 = &opt_int32;
alltypes.opt_int64 = &opt_int64;
alltypes.opt_uint32 = &opt_uint32;
alltypes.opt_uint64 = &opt_uint64;
alltypes.opt_sint32 = &opt_sint32;
alltypes.opt_sint64 = &opt_sint64;
alltypes.opt_bool = &opt_bool;
alltypes.opt_fixed32 = &opt_fixed32;
alltypes.opt_sfixed32 = &opt_sfixed32;
alltypes.opt_float = &opt_float;
alltypes.opt_fixed64 = &opt_fixed64;
alltypes.opt_sfixed64 = &opt_sfixed64;
alltypes.opt_double = &opt_double;
alltypes.opt_string = opt_string;
alltypes.opt_bytes = (pb_bytes_array_t*)&opt_bytes;
alltypes.opt_submsg = &opt_submsg;
alltypes.opt_enum = &opt_enum;
alltypes.opt_emptymsg = &opt_emptymsg;
}
alltypes.end = &end;
{
uint8_t buffer[4096];
pb_ostream_t stream = pb_ostream_from_buffer(buffer, sizeof(buffer));
/* Now encode it and check if we succeeded. */
if (pb_encode(&stream, AllTypes_fields, &alltypes))
{
SET_BINARY_MODE(stdout);
fwrite(buffer, 1, stream.bytes_written, stdout);
return 0; /* Success */
}
else
{
fprintf(stderr, "Encoding failed: %s\n", PB_GET_ERROR(&stream));
return 1; /* Failure */
}
}
}
bool Hal::CheckAndAct()
{
bool sendResult;
//Read the state of the microSwitch
if (microSwitch.isChanged()) {
debugPrint("Switch state: ");
debugPrintLn(microSwitch.ReadState());
uint8_t buf[128];
size_t message_length;
NodeMessage nodemsg = NodeMessage_init_zero;
/* Create a stream that will write to our buffer. */
pb_ostream_t stream = pb_ostream_from_buffer(buf, sizeof(buf));
nodemsg.reading.funcs.encode = &switchsensor_callback;
/* Now we are ready to encode the message! */
/* Then just check for any errors.. */
if (!pb_encode(&stream, NodeMessage_fields, &nodemsg))
{
debugPrint("Encoding failed: ");
debugPrintLn(PB_GET_ERROR(&stream));
}
else
{
message_length = stream.bytes_written;
debugPrint("message_length:");
debugPrintLn(message_length);
debugPrint("message:<");
for (uint8_t i = 0; i < message_length; i++)
{
debugPrint(buf[i], HEX);
if (i < message_length - 1)
debugPrint(" ");
}
debugPrintLn(">");
}
sendResult = HalImpl.sendMessage(buf, message_length);
// reset the Time Passed flag
alive.setCurrentTime();
}
//Read the state of the microSwitch
if (isMoved()) {
debugPrintLn("Box moved: ");
uint8_t buf[128];
size_t message_length;
NodeMessage nodemsg = NodeMessage_init_zero;
/* Create a stream that will write to our buffer. */
pb_ostream_t stream = pb_ostream_from_buffer(buf, sizeof(buf));
nodemsg.reading.funcs.encode = &movesensor_callback;
/* Now we are ready to encode the message! */
/* Then just check for any errors.. */
if (!pb_encode(&stream, NodeMessage_fields, &nodemsg))
{
debugPrint("Encoding failed: ");
debugPrintLn(PB_GET_ERROR(&stream));
}
else
{
message_length = stream.bytes_written;
debugPrint("message_length:");
debugPrintLn(message_length);
debugPrint("message:<");
for (uint8_t i = 0; i < message_length; i++)
{
debugPrint(buf[i], HEX);
if (i < message_length - 1)
debugPrint(" ");
}
debugPrintLn(">");
}
sendResult = HalImpl.sendMessage(buf, message_length);
hasMoved = false;
// reset the Time Passed flag
alive.setCurrentTime();
}
// check alive timer
if (alive.isTimePassed())
{
// reset the Time Passed flag
alive.resetTimePassed();
debugPrint("LTC values: mAh ");
debugPrint(ltc.getCharge(), 4);
debugPrint(F(" mAh\t"));
debugPrint(F("Current "));
debugPrint(ltc.getCurrent(), 4);
debugPrint(F(" A\t"));
debugPrint(F("Voltage "));
debugPrint(ltc.getVoltage(), 4);
debugPrint(F(" V\t"));
debugPrint(F("Temperature "));
debugPrint(ltc.getTemp(), 4);
debugPrint(F(" C\n"));
debugPrint("HTU values: ");
debugPrint(F("Temperature "));
debugPrint(htu.getTemp(), 4);
debugPrint(F(" C"));
debugPrint(F("Humidity "));
debugPrint(htu.getHum(), 4);
debugPrint(F(" %\n"));
uint8_t buf2[128];
size_t message_length;
NodeMessage nodemsg2 = NodeMessage_init_zero;
/* Create a stream that will write to our buffer. */
pb_ostream_t stream2 = pb_ostream_from_buffer(buf2, sizeof(buf2));
// add the temperature data to the output buffer
nodemsg2.reading.funcs.encode = &tempsensor_callback;
/* Now we are ready to encode the message! */
/* Then just check for any errors.. */
if (!pb_encode(&stream2, NodeMessage_fields, &nodemsg2))
{
debugPrint("Encoding failed: ");
debugPrintLn(PB_GET_ERROR(&stream2));
}
else
{
message_length = stream2.bytes_written;
debugPrint("message_length:");
debugPrintLn(message_length);
debugPrint("message:<");
for (uint8_t i = 0; i < message_length; i++)
{
debugPrint(buf2[i], HEX);
if (i < message_length - 1)
debugPrint(" ");
}
debugPrintLn(">");
}
// add the humidity data to the output buffer
nodemsg2.reading.funcs.encode = &humsensor_callback;
/* Now we are ready to encode the message! */
/* Then just check for any errors.. */
if (!pb_encode(&stream2, NodeMessage_fields, &nodemsg2))
{
debugPrint("Encoding failed: ");
debugPrintLn(PB_GET_ERROR(&stream2));
}
else
{
message_length = stream2.bytes_written;
debugPrint("message_length:");
debugPrintLn(message_length);
debugPrint("message:<");
for (uint8_t i = 0; i < message_length; i++)
{
debugPrint(buf2[i], HEX);
if (i < message_length - 1)
debugPrint(" ");
}
debugPrintLn(">");
}
// add the voltage data to the output buffer
nodemsg2.reading.funcs.encode = &voltsensor_callback;
/* Now we are ready to encode the message! */
/* Then just check for any errors.. */
if (!pb_encode(&stream2, NodeMessage_fields, &nodemsg2))
{
debugPrint("Encoding failed: ");
debugPrintLn(PB_GET_ERROR(&stream2));
}
else
{
message_length = stream2.bytes_written;
debugPrint("message_length:");
debugPrintLn(message_length);
debugPrint("message:<");
for (uint8_t i = 0; i < message_length; i++)
{
debugPrint(buf2[i], HEX);
if (i < message_length - 1)
debugPrint(" ");
}
debugPrintLn(">");
}
// add the current data to the output buffer
nodemsg2.reading.funcs.encode = ¤tsensor_callback;
/* Now we are ready to encode the message! */
/* Then just check for any errors.. */
if (!pb_encode(&stream2, NodeMessage_fields, &nodemsg2))
{
debugPrint("Encoding failed: ");
debugPrintLn(PB_GET_ERROR(&stream2));
}
else
{
message_length = stream2.bytes_written;
debugPrint("message_length:");
debugPrintLn(message_length);
debugPrint("message:<");
for (uint8_t i = 0; i < message_length; i++)
{
debugPrint(buf2[i], HEX);
if (i < message_length - 1)
debugPrint(" ");
}
debugPrintLn(">");
}
// add the charge data to the output buffer
nodemsg2.reading.funcs.encode = &chargesensor_callback;
/* Now we are ready to encode the message! */
/* Then just check for any errors.. */
if (!pb_encode(&stream2, NodeMessage_fields, &nodemsg2))
{
debugPrint("Encoding failed: ");
debugPrintLn(PB_GET_ERROR(&stream2));
}
else
{
message_length = stream2.bytes_written;
debugPrint("message_length:");
debugPrintLn(message_length);
debugPrint("message:<");
for (uint8_t i = 0; i < message_length; i++)
{
debugPrint(buf2[i], HEX);
if (i < message_length - 1)
debugPrint(" ");
}
debugPrintLn(">");
}
// only send the number of retries if it is > 0
if (getNumTxRetries() > 0)
{
// add the retry counter data to the output buffer
nodemsg2.reading.funcs.encode = &retriessensor_callback;
/* Now we are ready to encode the message! */
/* Then just check for any errors.. */
if (!pb_encode(&stream2, NodeMessage_fields, &nodemsg2))
{
debugPrint("Encoding failed: ");
debugPrintLn(PB_GET_ERROR(&stream2));
}
else
{
message_length = stream2.bytes_written;
debugPrint("message_length:");
debugPrintLn(message_length);
debugPrint("message:<");
for (uint8_t i = 0; i < message_length; i++)
{
debugPrint(buf2[i], HEX);
if (i < message_length - 1)
debugPrint(" ");
}
debugPrintLn(">");
}
}
// send the alive message
sendResult = HalImpl.sendMessage(buf2, message_length);
// if the send was succesfull then the retry counter can be reset
if (sendResult)
{
setNumTxRetries(0);
}
}
}
/* Oneofs */
static bool test_OneofMessage()
{
uint8_t buffer[256];
size_t msgsize;
{
pb_ostream_t stream = pb_ostream_from_buffer(buffer, sizeof(buffer));
/* Encode first with TestMessage */
{
OneofMessage msg = OneofMessage_init_zero;
msg.which_msgs = OneofMessage_msg1_tag;
fill_TestMessage(&msg.msgs.msg1);
if (!pb_encode(&stream, OneofMessage_fields, &msg))
{
fprintf(stderr, "Encode failed: %s\n", PB_GET_ERROR(&stream));
return false;
}
}
/* Encode second with SubMessage, invoking 'merge' behaviour */
{
OneofMessage msg = OneofMessage_init_zero;
msg.which_msgs = OneofMessage_msg2_tag;
msg.first = 999;
msg.msgs.msg2.dynamic_str = "ABCD";
msg.last = 888;
if (!pb_encode(&stream, OneofMessage_fields, &msg))
{
fprintf(stderr, "Encode failed: %s\n", PB_GET_ERROR(&stream));
return false;
}
}
msgsize = stream.bytes_written;
}
{
OneofMessage msg = OneofMessage_init_zero;
pb_istream_t stream = pb_istream_from_buffer(buffer, msgsize);
if (!pb_decode(&stream, OneofMessage_fields, &msg))
{
fprintf(stderr, "Decode failed: %s\n", PB_GET_ERROR(&stream));
return false;
}
TEST(msg.first == 999);
TEST(msg.which_msgs == OneofMessage_msg2_tag);
TEST(msg.msgs.msg2.dynamic_str);
TEST(strcmp(msg.msgs.msg2.dynamic_str, "ABCD") == 0);
TEST(msg.msgs.msg2.dynamic_str_arr == NULL);
TEST(msg.msgs.msg2.dynamic_submsg == NULL);
TEST(msg.last == 888);
pb_release(OneofMessage_fields, &msg);
TEST(get_alloc_count() == 0);
pb_release(OneofMessage_fields, &msg);
TEST(get_alloc_count() == 0);
}
return true;
}
void MotorController::setPID()
{
ros::Rate rate(frequency_);
ROS_INFO_STREAM("Setting PID Values:"
<< "\n\t P => L: " << p_l_ << " R: " << p_r_ << "\n\t D => L: " << d_l_ << " R: " << d_r_
<< "\n\t I => L: " << i_l_ << " R: " << i_r_ << "\n\t Kv => L: " << kv_l_ << " R: " << kv_r_);
bool valid_values = false; // pid values have been set correctly
/* This is the buffer where we will store the request message. */
uint8_t requestbuffer[256];
size_t message_length;
bool status;
/* allocate space for the request message to the server */
RequestMessage request = RequestMessage_init_zero;
/* Create a stream that will write to our buffer. */
pb_ostream_t ostream = pb_ostream_from_buffer(requestbuffer, sizeof(requestbuffer));
/* indicate that pid fields will contain values */
request.has_p_l = true;
request.has_p_r = true;
request.has_i_l = true;
request.has_i_r = true;
request.has_d_l = true;
request.has_d_r = true;
request.has_kv_l = true;
request.has_kv_r = true;
/* fill in the message fields */
request.p_l = static_cast<float>(p_l_);
request.p_r = static_cast<float>(p_r_);
request.i_l = static_cast<float>(i_l_);
request.i_r = static_cast<float>(i_r_);
request.d_l = static_cast<float>(d_l_);
request.d_r = static_cast<float>(d_r_);
request.kv_l = static_cast<float>(kv_l_);
request.kv_r = static_cast<float>(kv_r_);
/* encode the protobuffer */
status = pb_encode(&ostream, RequestMessage_fields, &request);
message_length = ostream.bytes_written;
/* check for any errors.. */
if (!status)
{
ROS_ERROR_STREAM("Encoding failed: " << PB_GET_ERROR(&ostream));
ros::shutdown();
}
size_t n; // n is the response from socket: 0 means connection closed, otherwise n = num bytes read
uint8_t buffer[256]; // buffer to read response into
// unsigned char responsebuffer[256];
/* Send PID values via ethernet and recieve response to ensure proper setting */
while (ros::ok() && !valid_values)
{
(*sock_).sendMessage(reinterpret_cast<char*>(requestbuffer), message_length);
memset(buffer, 0, sizeof(buffer));
n = (*sock_).readMessage(buffer); // blocks until data is read
// memcpy(responsebuffer, buffer, sizeof(responsebuffer));
if (n == 0)
{
ROS_ERROR_STREAM("Connection closed by server");
ros::shutdown();
}
/* Allocate space for the decoded message. */
ResponseMessage response = ResponseMessage_init_zero;
/* Create a stream that reads from the buffer. */
pb_istream_t istream = pb_istream_from_buffer(buffer, n);
/* decode the message. */
status = pb_decode(&istream, ResponseMessage_fields, &response);
/* check for any errors.. */
if (!status)
{
ROS_ERROR_STREAM("Decoding Failed: " << PB_GET_ERROR(&istream));
ros::shutdown();
}
valid_values = (response.p_l == static_cast<float>(p_l_)) && (response.p_r == static_cast<float>(p_r_)) &&
(response.i_l == static_cast<float>(i_l_)) && (response.i_r == static_cast<float>(i_r_)) &&
(response.d_l == static_cast<float>(d_l_)) && (response.d_r == static_cast<float>(d_r_)) &&
(response.kv_l == static_cast<float>(kv_l_)) && (response.kv_r == static_cast<float>(kv_r_));
rate.sleep();
}
ROS_INFO_ONCE("Sucessfully set all PID values");
}
int main()
{
/* This is the buffer where we will store our message. */
uint8_t buffer[128];
size_t message_length;
bool status;
/* Encode our message */
{
/* Allocate space on the stack to store the message data.
*
* Nanopb generates simple struct definitions for all the messages.
* - check out the contents of simple.pb.h!
* It is a good idea to always initialize your structures
* so that you do not have garbage data from RAM in there.
*/
SimpleMessage message = SimpleMessage_init_zero;
/* Create a stream that will write to our buffer. */
pb_ostream_t stream = pb_ostream_from_buffer(buffer, sizeof(buffer));
/* Fill in the lucky number */
message.lucky_number = 13;
message.unlucky.number = 42;
/* Now we are ready to encode the message! */
status = pb_encode(&stream, SimpleMessage_fields, &message);
message_length = stream.bytes_written;
/* Then just check for any errors.. */
if (!status)
{
printf("Encoding failed: %s\n", PB_GET_ERROR(&stream));
return 1;
}
}
/* Now we could transmit the message over network, store it in a file or
* wrap it to a pigeon's leg.
*/
/* But because we are lazy, we will just decode it immediately. */
{
/* Allocate space for the decoded message. */
SimpleMessage message = SimpleMessage_init_zero;
/* Create a stream that reads from the buffer. */
pb_istream_t stream = pb_istream_from_buffer(buffer, message_length);
/* Now we are ready to decode the message. */
status = pb_decode(&stream, SimpleMessage_fields, &message);
/* Check for errors... */
if (!status)
{
printf("Decoding failed: %s\n", PB_GET_ERROR(&stream));
return 1;
}
/* Print the data contained in the message. */
printf("Your lucky number was %d!\n", message.lucky_number);
printf("Your unlucky number was %u!\n", message.unlucky.number);
}
return 0;
}
