From 57cc9583a914de29f34ebdfd06d78663e4ae4045 Mon Sep 17 00:00:00 2001
From: Sergi Hernandez Juan <shernand@iri.upc.edu>
Date: Fri, 17 Feb 2023 10:54:30 +0100
Subject: [PATCH] Added functions to receive all the answers from bulk and sync
read commands.
---
include/dynamixelserver.h | 10 ++
src/dynamixelserver.cpp | 347 ++++++++++++++++++++++----------------
2 files changed, 215 insertions(+), 142 deletions(-)
diff --git a/include/dynamixelserver.h b/include/dynamixelserver.h
index c7266d3..3e9030a 100644
--- a/include/dynamixelserver.h
+++ b/include/dynamixelserver.h
@@ -106,11 +106,21 @@ class CDynamixelServer
*
*/
virtual unsigned char receive_status_packet_v1(unsigned char **data,unsigned char *len);
+ /**
+ * \brief
+ *
+ */
+ virtual void receive_sync_bulk_status_packets_v1(std::vector<unsigned char>& servo_ids, std::vector< std::vector<unsigned char> >& data);
/**
* \brief
*
*/
virtual unsigned char receive_status_packet_v2(unsigned char **data,unsigned short int *len);
+ /**
+ * \brief
+ *
+ */
+ virtual void receive_sync_bulk_status_packets_v2(std::vector<unsigned char>& servo_ids, std::vector< std::vector<unsigned char> >& data);
/**
* \brief
*
diff --git a/src/dynamixelserver.cpp b/src/dynamixelserver.cpp
index 37c91fc..a38ae2b 100644
--- a/src/dynamixelserver.cpp
+++ b/src/dynamixelserver.cpp
@@ -2,6 +2,7 @@
#include "dynamixelserver.h"
#include "eventexceptions.h"
#include <sstream>
+#include <iostream>
const unsigned short crc_table[256] = {
0x0000, 0x8005, 0x800F, 0x000A, 0x801B, 0x001E, 0x0014, 0x8011,
@@ -194,7 +195,7 @@ unsigned char CDynamixelServer::receive_status_packet_v1(unsigned char **data,un
do{
if((num=this->comm_dev->get_num_data())==0)
{
- this->event_server->wait_all(events,20);
+ this->event_server->wait_all(events,100);
num=this->comm_dev->get_num_data();
}
if((read+num)>1024)
@@ -262,6 +263,92 @@ unsigned char CDynamixelServer::receive_status_packet_v1(unsigned char **data,un
}
}
+
+void CDynamixelServer::receive_sync_bulk_status_packets_v1(std::vector<unsigned char>& servo_ids, std::vector< std::vector<unsigned char> >& data)
+{
+ std::list<std::string> events;
+ unsigned char data_int[1024];
+ int num=0,read=0,length=-1,new_data,total=0;
+ bool need_data=true;
+
+ if(this->comm_dev!=NULL)
+ {
+ try{
+ data.resize(servo_ids.size());
+ events.push_back(this->comm_dev->get_rx_event_id());
+ // read up to the length field
+ for(unsigned int i=0;i<servo_ids.size();)
+ {
+ if(need_data)
+ {
+ this->event_server->wait_all(events,100);
+ new_data=this->comm_dev->get_num_data();
+ this->comm_dev->read(&data_int[total],new_data);
+ total+=new_data;
+ num+=new_data;
+ need_data=false;
+ }
+ if(length==-1)
+ {
+ if(num>=4)
+ {
+ if(data_int[read+2]!=servo_ids[i])
+ throw CDynamixelServerException(_HERE_,"Invalid status packet order in Sync/Bulk read");
+ length=data_int[read+3]+4;
+ }
+ else
+ need_data=true;
+ }
+ if(length!=-1 && num>=length)
+ {
+ if(this->compute_checksum_v1(&data_int[read],length)!=0x00)
+ {
+ data[i].clear();
+ read+=length;
+ num-=length;
+ length=-1;
+ i++;
+ continue;
+ }
+ try{
+ this->handle_error(data_int[read+4]);
+ }catch(CException &e){
+ data[i].clear();
+ read+=length;
+ num-=length;
+ length=-1;
+ i++;
+ continue;
+ }
+ if(length>6)
+ {
+ data[i].resize(length-6);
+ for(unsigned j=0;j<(unsigned int)(length-6);j++)
+ data[i][j]=data_int[read+5+j];
+ }
+ else
+ data[i].clear();
+ read+=length;
+ num-=length;
+ length=-1;
+ i++;
+ }
+ else
+ need_data=true;
+ }
+ }catch(CEventTimeoutException &e){
+ throw e;
+ }catch(CException &e){
+ throw e;
+ }
+ }
+ else
+ {
+ /* handle exceptions */
+ throw CDynamixelServerException(_HERE_,"The communication device is not properly configured");
+ }
+}
+
unsigned char CDynamixelServer::receive_status_packet_v2(unsigned char **data,unsigned short int *len)
{
std::list<std::string> events;
@@ -277,7 +364,7 @@ unsigned char CDynamixelServer::receive_status_packet_v2(unsigned char **data,un
do{
if((num=this->comm_dev->get_num_data())==0)
{
- this->event_server->wait_all(events,20);
+ this->event_server->wait_all(events,100);
num=this->comm_dev->get_num_data();
}
if((read+num)>1024)
@@ -298,7 +385,7 @@ unsigned char CDynamixelServer::receive_status_packet_v2(unsigned char **data,un
{
if((num=this->comm_dev->get_num_data())==0)
{
- this->event_server->wait_all(events,20);
+ this->event_server->wait_all(events,100);
num=this->comm_dev->get_num_data();
}
if((read-start+num)>length)
@@ -345,6 +432,95 @@ unsigned char CDynamixelServer::receive_status_packet_v2(unsigned char **data,un
}
}
+void CDynamixelServer::receive_sync_bulk_status_packets_v2(std::vector<unsigned char> &servo_ids,std::vector< std::vector<unsigned char> > &data)
+{
+ std::list<std::string> events;
+ unsigned char data_int[1024];
+ unsigned short int crc;
+ int num=0,read=0,length=-1,new_data,total=0;
+ bool need_data=true;
+
+ if(this->comm_dev!=NULL)
+ {
+ try{
+ data.resize(servo_ids.size());
+ events.push_back(this->comm_dev->get_rx_event_id());
+ // read up to the length field
+ for(unsigned int i=0;i<servo_ids.size();)
+ {
+ if(need_data)
+ {
+ this->event_server->wait_all(events,100);
+ new_data=this->comm_dev->get_num_data();
+ this->comm_dev->read(&data_int[total],new_data);
+ total+=new_data;
+ num+=new_data;
+ need_data=false;
+ }
+ if(length==-1)
+ {
+ if(num>=7)
+ {
+ if(data_int[read+2]!=servo_ids[i])
+ throw CDynamixelServerException(_HERE_,"Invalid status packet order in Sync/Bulk read");
+ length=data_int[read+5]+data_int[read+6]*256+7;
+ }
+ else
+ need_data=true;
+ }
+ if(length!=-1 && num>=length)
+ {
+ // check the checksum
+ crc=this->compute_checksum_v2(&data_int[read],length-2);
+ if((crc%256)!=data_int[read+length-2] || (crc/256)!=data_int[read+length-1])
+ {
+ /* handle exceptions */
+ data[i].clear();
+ read+=length;
+ num-=length;
+ length=-1;
+ i++;
+ continue;
+ }
+ try{
+ this->handle_error(data_int[read+8]);
+ }catch(CException &e){
+ data[i].clear();
+ read+=length;
+ num-=length;
+ length=-1;
+ i++;
+ continue;
+ }
+ if(length>11)
+ {
+ data[i].resize(length-11);
+ for(unsigned j=0;j<(unsigned int)(length-11);j++)
+ data[i][j]=data_int[read+9+j];
+ }
+ else
+ data[i].clear();
+ read+=length;
+ num-=length;
+ length=-1;
+ i++;
+ }
+ else
+ need_data=true;
+ }
+ }catch(CEventTimeoutException &e){
+ throw e;
+ }catch(CException &e){
+ throw e;
+ }
+ }
+ else
+ {
+ /* handle exceptions */
+ throw CDynamixelServerException(_HERE_,"The communication device is not properly configured");
+ }
+}
+
unsigned char CDynamixelServer::compute_checksum_v1(unsigned char *packet,int len)
{
int i=0;
@@ -895,6 +1071,7 @@ void CDynamixelServer::write_sync(std::vector<unsigned char>& servo_ids,unsigned
throw CDynamixelServerException(_HERE_,"Too much data blocks or some missing servos");
}
try{
+ this->dynamixel_access.enter();
if(version==dyn_version1)
{
length=2+(data[0].size()+1)*servo_ids.size();
@@ -909,9 +1086,7 @@ void CDynamixelServer::write_sync(std::vector<unsigned char>& servo_ids,unsigned
}
try
{
- this->dynamixel_access.enter();
this->send_instruction_packet_v1(dyn_sync_write,data_int,length);
- this->dynamixel_access.exit();
}
catch(CEventTimeoutException &e)
{
@@ -941,9 +1116,7 @@ void CDynamixelServer::write_sync(std::vector<unsigned char>& servo_ids,unsigned
}
try
{
- this->dynamixel_access.enter();
this->send_instruction_packet_v2(dyn_sync_write,data_int,length);
- this->dynamixel_access.exit();
}
catch(CEventTimeoutException &e)
{
@@ -957,6 +1130,7 @@ void CDynamixelServer::write_sync(std::vector<unsigned char>& servo_ids,unsigned
}
delete[] data_int;
}
+ this->dynamixel_access.exit();
}catch(CException &e){
if(data_int!=NULL)
{
@@ -969,9 +1143,8 @@ void CDynamixelServer::write_sync(std::vector<unsigned char>& servo_ids,unsigned
void CDynamixelServer::read_sync(std::vector<unsigned char>& servo_ids,unsigned short int start_addr,unsigned short int length, std::vector< std::vector<unsigned char> >& data,dyn_version_t version)
{
- unsigned char *data_int=NULL,*data_out=NULL,error,length_v1;
- unsigned short length_v2;
- unsigned int i,j=0;
+ unsigned char *data_int=NULL;
+ unsigned int i;
int length_int=0;
if(servo_ids.size()==0)
@@ -980,6 +1153,7 @@ void CDynamixelServer::read_sync(std::vector<unsigned char>& servo_ids,unsigned
throw CDynamixelServerException(_HERE_,"No servos specified");
}
try{
+ this->dynamixel_access.enter();
if(version==dyn_version2)
{
length_int=4+servo_ids.size();
@@ -992,38 +1166,9 @@ void CDynamixelServer::read_sync(std::vector<unsigned char>& servo_ids,unsigned
data_int[4+i]=servo_ids[i];
try
{
- this->dynamixel_access.enter();
this->send_instruction_packet_v2(dyn_sync_read,data_int,length_int);
/* read all the data from all the servos */
- if(data_int!=NULL)
- {
- delete[] data_int;
- data_int=NULL;
- }
- data.resize(servo_ids.size());
- for(i=0;i<servo_ids.size();i++)
- {
- try{
- error=this->receive_status_packet_v2(&data_out,&length_v2);
- this->handle_error(error);
- if(data_out!=NULL)
- {
- data[i].resize(length_v2);
- for(j=0;j<length_v2;j++)
- data[i][j]=data_out[j];
- delete[] data_out;
- data_out=NULL;
- }
- }catch(CException &e){
- if(data_out!=NULL)
- {
- delete[] data_out;
- data_out=NULL;
- }
- throw e;
- }
- }
- this->dynamixel_access.exit();
+ receive_sync_bulk_status_packets_v2(servo_ids,data);
}
catch(CEventTimeoutException &e)
{
@@ -1035,6 +1180,7 @@ void CDynamixelServer::read_sync(std::vector<unsigned char>& servo_ids,unsigned
this->dynamixel_access.exit();
throw e;
}
+ delete[] data_int;
}
else
{
@@ -1046,38 +1192,9 @@ void CDynamixelServer::read_sync(std::vector<unsigned char>& servo_ids,unsigned
data_int[2+i]=servo_ids[i];
try
{
- this->dynamixel_access.enter();
this->send_instruction_packet_v1(dyn_sync_read,data_int,length_int);
/* read all the data from all the servos */
- if(data_int!=NULL)
- {
- delete[] data_int;
- data_int=NULL;
- }
- data.resize(servo_ids.size());
- for(i=0;i<servo_ids.size();i++)
- {
- try{
- error=this->receive_status_packet_v1(&data_out,&length_v1);
- this->handle_error(error);
- if(data_out!=NULL)
- {
- data[i].resize(length_v1);
- for(j=0;j<length_v1;j++)
- data[i][j]=data_out[j];
- delete[] data_out;
- data_out=NULL;
- }
- }catch(CException &e){
- if(data_out!=NULL)
- {
- delete[] data_out;
- data_out=NULL;
- }
- throw e;
- }
- }
- this->dynamixel_access.exit();
+ receive_sync_bulk_status_packets_v1(servo_ids,data);
}
catch(CEventTimeoutException &e)
{
@@ -1089,12 +1206,14 @@ void CDynamixelServer::read_sync(std::vector<unsigned char>& servo_ids,unsigned
this->dynamixel_access.exit();
throw e;
}
+ delete[] data_int;
}
+ this->dynamixel_access.exit();
}catch(CException &e){
if(data_int!=NULL)
{
delete[] data_int;
- data_out=NULL;
+ data_int=NULL;
}
throw e;
}
@@ -1122,6 +1241,7 @@ void CDynamixelServer::write_bulk(std::vector<unsigned char>& servo_ids,std::vec
throw CDynamixelServerException(_HERE_,"Too many or not enough data blocks");
}
try{
+ this->dynamixel_access.enter();
if(version==dyn_version2)
{
length=0;
@@ -1141,9 +1261,7 @@ void CDynamixelServer::write_bulk(std::vector<unsigned char>& servo_ids,std::vec
}
try
{
- this->dynamixel_access.enter();
this->send_instruction_packet_v2(dyn_bulk_write,data_int,length);
- this->dynamixel_access.exit();
}
catch(CEventTimeoutException &e)
{
@@ -1174,9 +1292,7 @@ void CDynamixelServer::write_bulk(std::vector<unsigned char>& servo_ids,std::vec
}
try
{
- this->dynamixel_access.enter();
this->send_instruction_packet_v1(dyn_bulk_write,data_int,length);
- this->dynamixel_access.exit();
}
catch(CEventTimeoutException &e)
{
@@ -1190,6 +1306,7 @@ void CDynamixelServer::write_bulk(std::vector<unsigned char>& servo_ids,std::vec
}
delete[] data_int;
}
+ this->dynamixel_access.exit();
}catch(CException &e){
if(data_int!=NULL)
{
@@ -1202,9 +1319,8 @@ void CDynamixelServer::write_bulk(std::vector<unsigned char>& servo_ids,std::vec
void CDynamixelServer::read_bulk(std::vector<unsigned char>& servo_ids,std::vector<unsigned short int> &start_addr,std::vector<unsigned short int> &length, std::vector< std::vector<unsigned char> >& data,dyn_version_t version)
{
- unsigned char *data_int=NULL,*data_out=NULL,error,length_v1;
- unsigned short int length_v2;
- unsigned int i,j=0;
+ unsigned char *data_int=NULL;
+ unsigned int i;
int length_int=0;
if(servo_ids.size()==0)
@@ -1223,6 +1339,7 @@ void CDynamixelServer::read_bulk(std::vector<unsigned char>& servo_ids,std::vect
throw CDynamixelServerException(_HERE_,"Too many or not enough read lengths");
}
try{
+ this->dynamixel_access.enter();
if(version==dyn_version2)
{
length_int=5*servo_ids.size();
@@ -1237,38 +1354,9 @@ void CDynamixelServer::read_bulk(std::vector<unsigned char>& servo_ids,std::vect
}
try
{
- this->dynamixel_access.enter();
this->send_instruction_packet_v2(dyn_bulk_read,data_int,length_int);
/* read all the data from all the servos */
- if(data_int!=NULL)
- {
- delete[] data_int;
- data_int=NULL;
- }
- data.resize(servo_ids.size());
- for(i=0;i<servo_ids.size();i++)
- {
- error=this->receive_status_packet_v2(&data_out,&length_v2);
- try{
- this->handle_error(error);
- if(data_out!=NULL)
- {
- data[i].resize(length_v2);
- for(j=0;j<length_v2;j++)
- data[i][j]=data_out[j];
- delete[] data_out;
- data_out=NULL;
- }
- }catch(CException &e){
- if(data_out!=NULL)
- {
- delete[] data_out;
- data_out=NULL;
- }
- throw e;
- }
- }
- this->dynamixel_access.exit();
+ receive_sync_bulk_status_packets_v2(servo_ids,data);
}
catch(CEventTimeoutException &e)
{
@@ -1280,51 +1368,24 @@ void CDynamixelServer::read_bulk(std::vector<unsigned char>& servo_ids,std::vect
this->dynamixel_access.exit();
throw e;
}
+ delete[] data_int;
}
else
{
- length_int=3*servo_ids.size();
+ length_int=3*servo_ids.size()+1;
data_int=new unsigned char[length_int];
+ data_int[0]=0x00;
for(i=0;i<servo_ids.size();i++)
{
- data_int[i*3]=servo_ids[i];
- data_int[i*3+1]=(unsigned char)start_addr[i];
- data_int[i*3+2]=(unsigned char)length[i];
+ data_int[i*3+1]=(unsigned char)length[i];
+ data_int[i*3+2]=servo_ids[i];
+ data_int[i*3+3]=(unsigned char)start_addr[i];
}
try
{
- this->dynamixel_access.enter();
this->send_instruction_packet_v1(dyn_bulk_read,data_int,length_int);
/* read all the data from all the servos */
- if(data_int!=NULL)
- {
- delete[] data_int;
- data_int=NULL;
- }
- data.resize(servo_ids.size());
- for(i=0;i<servo_ids.size();i++)
- {
- error=this->receive_status_packet_v1(&data_out,&length_v1);
- try{
- this->handle_error(error);
- if(data_out!=NULL)
- {
- data[i].resize(length_v1);
- for(j=0;j<length_v1;j++)
- data[i][j]=data_out[j];
- delete[] data_out;
- data_out=NULL;
- }
- }catch(CException &e){
- if(data_out!=NULL)
- {
- delete[] data_out;
- data_out=NULL;
- }
- throw e;
- }
- }
- this->dynamixel_access.exit();
+ receive_sync_bulk_status_packets_v1(servo_ids,data);
}
catch(CEventTimeoutException &e)
{
@@ -1336,7 +1397,9 @@ void CDynamixelServer::read_bulk(std::vector<unsigned char>& servo_ids,std::vect
this->dynamixel_access.exit();
throw e;
}
+ delete[] data_int;
}
+ this->dynamixel_access.exit();
}catch(CException &e){
if(data_int!=NULL)
{
--
GitLab