CDxlROSPacketHandler.cpp
Go to the documentation of this file.
00001 /*
00002  * CDxlROSPacketHandler.cpp
00003  *
00004  *      Author: Wouter Caarls
00005  */
00006 
00007 #include <threemxl/platform/hardware/dynamixel/CDxlCom.h>
00008 #include <threemxl/CDxlROSPacketHandler.h>
00009 #include <shared_serial/SendTo.h>
00010 #include <shared_serial/Recv.h>
00011 #include <shared_serial/Flush.h>
00012 
00013 CDxlROSPacketHandler::CDxlROSPacketHandler(const char *path) :
00014   nh_(path), initialized_(false), socket_(0), last_error_(0)
00015 {
00016 }
00017 
00018 int CDxlROSPacketHandler::init()
00019 { 
00020   if (!initialized_)
00021   {
00022     ROS_INFO("Registering service clients");
00023   
00024     sendto_service_ = nh_.serviceClient<shared_serial::SendTo>("sendto");
00025     sendto_service_.waitForExistence();
00026   
00027     recv_service_ = nh_.serviceClient<shared_serial::Recv>("recv");
00028     recv_service_.waitForExistence();
00029 
00030     flush_pub_ = nh_.advertise<shared_serial::Flush>("flush", 10);
00031   
00032     initialized_ = true;
00033   }
00034   
00035   return DXL_SUCCESS;
00036 }
00037 
00038 int CDxlROSPacketHandler::sendPacket(CDxlPacket *packet, bool replyExpected)
00039 {
00040   if (!initialized_) init();
00041 
00042   int length = packet->length();
00043   BYTE *data = packet->data();
00044 
00045   shared_serial::SendTo srv;
00046 
00047   srv.request.socket = 0;  
00048   srv.request.data.resize(length);    
00049   for (int ii=0; ii != length; ++ii)
00050     srv.request.data[ii] = data[ii];
00051   srv.request.timeout = 1;
00052   
00053   for (int ii=0; ii != SEND_RETRY_FACTOR; ++ii)
00054   {
00055     if (sendto_service_.call(srv))
00056     {
00057       socket_ = srv.response.socket;
00058       return DXL_SUCCESS;
00059     }
00060     
00061     ROS_WARN("Error sending packet");
00062     usleep(1000);
00063   }
00064   
00065   socket_ = 0;
00066   last_error_ = 0;
00067   ROS_ERROR("Couldn't send packet");
00068   return DXL_PKT_SEND_ERROR;
00069 }
00070 
00071 int CDxlROSPacketHandler::receivePacketWait(CDxlStatusPacket *packet, int seconds, int microseconds)
00072 {
00073   if (!initialized_) init();
00074 
00075   shared_serial::Recv srv;
00076   
00077   unsigned int length = packet->length();
00078   BYTE *data = packet->data();
00079   
00080   srv.request.socket = socket_;
00081   srv.request.length = length;
00082   srv.request.recv_timeout = seconds+microseconds/1000000.;
00083   srv.request.sock_timeout = 0;
00084   
00085   if (!recv_service_.call(srv))
00086   {
00087     socket_ = 0;
00088     last_error_ = 0;
00089     
00090     ROS_WARN("Couldn't receive packet");
00091     return DXL_PKT_RECV_ERROR;
00092   }
00093   
00094   socket_ = 0;
00095   for (unsigned int ii=0; ii != srv.response.data.size(); ++ii)
00096     data[ii] = srv.response.data[ii];
00097     
00098   if (srv.response.data.size() != length)
00099   {
00100     last_error_ = srv.response.data.size();
00101     ROS_WARN("Short reply");
00102     return DXL_PKT_RECV_ERROR;
00103   }
00104 
00105   if (packet->calcChecksum() != packet->readChecksum())
00106   {
00107     last_error_ = srv.response.data.size();
00108     ROS_WARN_STREAM("Checksum error in packet: Received CS:"<< std::setw (2) << std::uppercase << std::hex << (int)packet->readChecksum() << " Calculated CS:"<< std::hex << (int)packet->calcChecksum() << std::dec);
00109     ROS_WARN_STREAM("Packet was " << packet->getPktString());
00110     
00111     shared_serial::Flush msg;
00112     msg.socket = 0;
00113     msg.timeout = 0;
00114     flush_pub_.publish(msg);
00115     
00116     return DXL_PKT_RECV_CHECKSUM_ERR;
00117   }
00118   
00119   return DXL_SUCCESS;
00120 }
00121 
00122 int CDxlROSPacketHandler::getLastError()
00123 {
00124   return last_error_;
00125 }


threemxl
Author(s):
autogenerated on Fri Aug 28 2015 13:21:08