Head

GCC Code Coverage Report

Directory:	./		Exec	Total	Coverage
File:	src/catkin/hardware/ati_ft_sensor/src/AtiFTSensor.cpp	Lines:	1	116	0.9 %
Date:	2020-04-15 11:50:02	Branches:	2	44	4.5 %


/*
 * AtiFTSensor.cpp
 *
 *  Created on: Oct 22, 2013
 *      Author: righetti
 */

#include <errno.h>
#include <unistd.h>
#include <string>
#include <cstdio>
#ifdef XENOMAI
#include <rtnet.h>
#include <native/timer.h>
#else
// Define xenomai like API for normal linux using defines.
#define rt_dev_socket socket
#define rt_dev_ioctl ioctl
#define rt_dev_close close
#define rt_dev_setsockopt setsockopt
#define rt_dev_bind bind
#define rt_dev_recv recv
#define rt_dev_recvmsg recvmsg
#define rt_dev_send send
#define rt_dev_sendto sendto
#define rt_dev_connect connect

#define rt_mutex_acquire(mutex, d) ((mutex)->lock())
#define rt_mutex_release(mutex) ((mutex)->unlock())

#endif
#include <AtiFTSensor.h>

namespace ati_ft_sensor
{
AtiFTSensor::AtiFTSensor()
{
  initialized_ = false;
}

bool AtiFTSensor::initialize()
{
  if(initialized_)
  {
    printf("warning already initialized\n");
    return true;
  }
  printf("initializing\n");

  //init some values
  for(int i=0; i<3; ++i)
  {
    F_bias_[i] = 0.0;
    T_bias_[i] = 0.0;
  }
  initialized_ = false;
  going_ = true;
  streaming_ = false;

  //setup the networking sockets
  memset(&local_address_, 0, sizeof(struct sockaddr_in));
  memset(&remote_address_, 0, sizeof(struct sockaddr_in));
  local_address_.sin_family = AF_INET;
  local_address_.sin_addr.s_addr = INADDR_ANY;
  local_address_.sin_port = htons(49152);

  remote_address_.sin_family = AF_INET;
  inet_aton("192.168.4.1",&(remote_address_.sin_addr));
  remote_address_.sin_port = htons(49152);

  socket_ = rt_dev_socket(AF_INET, SOCK_DGRAM, 0);
  if(socket_<0)
  {
    printf("cannot create socket, error %d, %s\n",errno, strerror(errno));
    return false;
  }
  if(rt_dev_bind(socket_, (struct sockaddr *) &local_address_, sizeof(struct sockaddr_in)) < 0)
  {
    printf("cannot bind socket, error: %d, %s", errno, strerror(errno));
    return false;
  }
  if(rt_dev_connect(socket_, (struct sockaddr *) &remote_address_, sizeof(struct sockaddr_in)) < 0)
  {
    printf("cannot connect socket, error: %d, %s", errno, strerror(errno));
    return false;
  }

  printf("created sockets\n");

#ifdef XENOMAI
  if(rt_pipe_create(&stream_pipe_, "ati_ft_stream",P_MINOR_AUTO,0))
  {
    printf("cannot create pipe, error: %d, %s", errno, strerror(errno));
    return false;
  }

  //create mutex for the threads
  rt_mutex_create(&mutex_,NULL);
#endif

  //now create the reading thread
  reading_thread_.create_realtime_thread(
    &ati_ft_sensor::AtiFTSensor::read_ft, this);

  initialized_ = true;
  return initialized_;
}

void AtiFTSensor::read_ft()
{
  //send message to start streaming
  send_msg msg;
  msg.command_header = htons(0x1234);
  msg.command = htons(0x0002);
  msg.sample_count = 0;
  rt_dev_send(socket_, &msg, sizeof(msg), 0);

  //some internal variables
  bool internal_going = true;

#ifdef XENOMAI
  RTIME time1, time2;
  time2 = rt_timer_read();
#endif

  //the main reading loop
  while(internal_going)
  {
#ifdef XENOMAI
    time1 = rt_timer_read();
#endif

    //read the socket
    received_msg rcv_msg;
    ssize_t response = rt_dev_recv(socket_, &rcv_msg, sizeof(rcv_msg), 0);
    if(response != sizeof(rcv_msg))
    {
      printf("Received message of unexpected length %ld\n", response);
    }

    //update state / first get the byte order right
    rt_mutex_acquire(&mutex_,TM_INFINITE);
    rdt_sequence_ = ntohl(rcv_msg.rdt_sequence);
    ft_sequence_ = ntohl(rcv_msg.ft_sequence);
    status_ = ntohl(rcv_msg.status);
    rcv_msg.Fx = ntohl(rcv_msg.Fx);
    rcv_msg.Fy = ntohl(rcv_msg.Fy);
    rcv_msg.Fz = ntohl(rcv_msg.Fz);
    rcv_msg.Tx = ntohl(rcv_msg.Tx);
    rcv_msg.Ty = ntohl(rcv_msg.Ty);
    rcv_msg.Tz = ntohl(rcv_msg.Tz);
    F_[0] = double(rcv_msg.Fx)/count_per_force_ - F_bias_[0];
    F_[1] = double(rcv_msg.Fy)/count_per_force_ - F_bias_[1];
    F_[2] = double(rcv_msg.Fz)/count_per_force_ - F_bias_[2];
    T_[0] = double(rcv_msg.Tx)/count_per_torque_ - T_bias_[0];
    T_[1] = double(rcv_msg.Ty)/count_per_torque_ - T_bias_[1];
    T_[2] = double(rcv_msg.Tz)/count_per_torque_ - T_bias_[2];

    //if streaming mode, copy to pipe
    if(streaming_)
    {
#ifdef XENOMAI
      steaming_msg log;
      log.rdt_seq = rdt_sequence_;
      log.ft_seq = ft_sequence_;
      log.status = status_;
      for(int i=0; i<3; ++i)
      {
        log.F[i] = F_[i];
        log.T[i] = T_[i];
      }
      log.time = double(time1-time2)/10e9;
      time2 = time1;
      rt_pipe_write(&stream_pipe_,&log,sizeof(log), P_NORMAL);
#else
      printf("Streaming is only supported on xenomai.\n");
      exit(-1);
#endif
    }
    internal_going = going_;
    rt_mutex_release(&mutex_);
  }
  //stop streaming
  msg.command = 0x0000;
  rt_dev_send(socket_, &msg, sizeof(msg), 0);
}

void AtiFTSensor::setBias()
{
  double* force = NULL;
  double* torque = NULL;
  setBias(force, torque);
}

void AtiFTSensor::setBias(double* force, double* torque)
{
  rt_mutex_acquire(&mutex_,TM_INFINITE);
  for(int i=0; i<3; ++i) {
    if (force == NULL) {
      F_bias_[i] = F_[i];
    } else {
      F_bias_[i] = force[i];
    }
    if (torque == NULL) {
      T_bias_[i] = T_[i];
    } else {
      T_bias_[i] = torque[i];
    }
  }
  rt_mutex_release(&mutex_);
}

void AtiFTSensor::resetBias()
{
  rt_mutex_acquire(&mutex_,TM_INFINITE);
  for(int i=0; i<3; ++i)
  {
    F_bias_[i] = 0.;
    T_bias_[i] = 0.;
  }
  rt_mutex_release(&mutex_);
}


void AtiFTSensor::getStatus(uint32_t& rdt_seq, uint32_t& ft_seq, uint32_t& status)
{
  rt_mutex_acquire(&mutex_,TM_INFINITE);

  rdt_seq = rdt_sequence_;
  ft_seq = ft_sequence_;
  status = status_;

  rt_mutex_release(&mutex_);
}

void AtiFTSensor::getFT(double* force, double* torque)
{
  rt_mutex_acquire(&mutex_,TM_INFINITE);

  for(int i=0; i<3; ++i)
  {
    force[i] = F_[i];
    torque[i] = T_[i];
  }

  rt_mutex_release(&mutex_);
}

void AtiFTSensor::stop()
{
  if(initialized_)
  {
    rt_mutex_acquire(&mutex_,TM_INFINITE);
    going_ = false;
    rt_mutex_release(&mutex_);
    reading_thread_.join();
    rt_dev_close(socket_);
#ifdef XENOMAI
    rt_pipe_delete(&stream_pipe_);
#endif
    initialized_ = false;
  }
}

void AtiFTSensor::stream(bool stream)
{
  rt_mutex_acquire(&mutex_,TM_INFINITE);
  streaming_ = stream;
  rt_mutex_release(&mutex_);
}

AtiFTSensor::~AtiFTSensor()
{
  stop();
}

}


Generated by: GCOVR (Version 4.2)

Line	Branch	Exec	Source
1			/*
2			* AtiFTSensor.cpp
3			*
4			* Created on: Oct 22, 2013
5			* Author: righetti
6			*/
7
8			#include <errno.h>
9			#include <unistd.h>
10			#include <string>
11			#include <cstdio>
12			#ifdef XENOMAI
13			#include <rtnet.h>
14			#include <native/timer.h>
15			#else
16			// Define xenomai like API for normal linux using defines.
17			#define rt_dev_socket socket
18			#define rt_dev_ioctl ioctl
19			#define rt_dev_close close
20			#define rt_dev_setsockopt setsockopt
21			#define rt_dev_bind bind
22			#define rt_dev_recv recv
23			#define rt_dev_recvmsg recvmsg
24			#define rt_dev_send send
25			#define rt_dev_sendto sendto
26			#define rt_dev_connect connect
27
28			#define rt_mutex_acquire(mutex, d) ((mutex)->lock())
29			#define rt_mutex_release(mutex) ((mutex)->unlock())
30
31			#endif
32			#include <AtiFTSensor.h>
33
34			namespace ati_ft_sensor
35			{
36			AtiFTSensor::AtiFTSensor()
37			{
38			initialized_ = false;
39			}
40
41			bool AtiFTSensor::initialize()
42			{
43			if(initialized_)
44			{
45			printf("warning already initialized\n");
46			return true;
47			}
48			printf("initializing\n");
49
50			//init some values
51			for(int i=0; i<3; ++i)
52			{
53			F_bias_[i] = 0.0;
54			T_bias_[i] = 0.0;
55			}
56			initialized_ = false;
57			going_ = true;
58			streaming_ = false;
59
60			//setup the networking sockets
61			memset(&local_address_, 0, sizeof(struct sockaddr_in));
62			memset(&remote_address_, 0, sizeof(struct sockaddr_in));
63			local_address_.sin_family = AF_INET;
64			local_address_.sin_addr.s_addr = INADDR_ANY;
65			local_address_.sin_port = htons(49152);
66
67			remote_address_.sin_family = AF_INET;
68			inet_aton("192.168.4.1",&(remote_address_.sin_addr));
69			remote_address_.sin_port = htons(49152);
70
71			socket_ = rt_dev_socket(AF_INET, SOCK_DGRAM, 0);
72			if(socket_<0)
73			{
74			printf("cannot create socket, error %d, %s\n",errno, strerror(errno));
75			return false;
76			}
77			if(rt_dev_bind(socket_, (struct sockaddr *) &local_address_, sizeof(struct sockaddr_in)) < 0)
78			{
79			printf("cannot bind socket, error: %d, %s", errno, strerror(errno));
80			return false;
81			}
82			if(rt_dev_connect(socket_, (struct sockaddr *) &remote_address_, sizeof(struct sockaddr_in)) < 0)
83			{
84			printf("cannot connect socket, error: %d, %s", errno, strerror(errno));
85			return false;
86			}
87
88			printf("created sockets\n");
89
90			#ifdef XENOMAI
91			if(rt_pipe_create(&stream_pipe_, "ati_ft_stream",P_MINOR_AUTO,0))
92			{
93			printf("cannot create pipe, error: %d, %s", errno, strerror(errno));
94			return false;
95			}
96
97			//create mutex for the threads
98			rt_mutex_create(&mutex_,NULL);
99			#endif
100
101			//now create the reading thread
102			reading_thread_.create_realtime_thread(
103			&ati_ft_sensor::AtiFTSensor::read_ft, this);
104
105			initialized_ = true;
106			return initialized_;
107			}
108
109			void AtiFTSensor::read_ft()
110			{
111			//send message to start streaming
112			send_msg msg;
113			msg.command_header = htons(0x1234);
114			msg.command = htons(0x0002);
115			msg.sample_count = 0;
116			rt_dev_send(socket_, &msg, sizeof(msg), 0);
117
118			//some internal variables
119			bool internal_going = true;
120
121			#ifdef XENOMAI
122			RTIME time1, time2;
123			time2 = rt_timer_read();
124			#endif
125
126			//the main reading loop
127			while(internal_going)
128			{
129			#ifdef XENOMAI
130			time1 = rt_timer_read();
131			#endif
132
133			//read the socket
134			received_msg rcv_msg;
135			ssize_t response = rt_dev_recv(socket_, &rcv_msg, sizeof(rcv_msg), 0);
136			if(response != sizeof(rcv_msg))
137			{
138			printf("Received message of unexpected length %ld\n", response);
139			}
140
141			//update state / first get the byte order right
142			rt_mutex_acquire(&mutex_,TM_INFINITE);
143			rdt_sequence_ = ntohl(rcv_msg.rdt_sequence);
144			ft_sequence_ = ntohl(rcv_msg.ft_sequence);
145			status_ = ntohl(rcv_msg.status);
146			rcv_msg.Fx = ntohl(rcv_msg.Fx);
147			rcv_msg.Fy = ntohl(rcv_msg.Fy);
148			rcv_msg.Fz = ntohl(rcv_msg.Fz);
149			rcv_msg.Tx = ntohl(rcv_msg.Tx);
150			rcv_msg.Ty = ntohl(rcv_msg.Ty);
151			rcv_msg.Tz = ntohl(rcv_msg.Tz);
152			F_[0] = double(rcv_msg.Fx)/count_per_force_ - F_bias_[0];
153			F_[1] = double(rcv_msg.Fy)/count_per_force_ - F_bias_[1];
154			F_[2] = double(rcv_msg.Fz)/count_per_force_ - F_bias_[2];
155			T_[0] = double(rcv_msg.Tx)/count_per_torque_ - T_bias_[0];
156			T_[1] = double(rcv_msg.Ty)/count_per_torque_ - T_bias_[1];
157			T_[2] = double(rcv_msg.Tz)/count_per_torque_ - T_bias_[2];
158
159			//if streaming mode, copy to pipe
160			if(streaming_)
161			{
162			#ifdef XENOMAI
163			steaming_msg log;
164			log.rdt_seq = rdt_sequence_;
165			log.ft_seq = ft_sequence_;
166			log.status = status_;
167			for(int i=0; i<3; ++i)
168			{
169			log.F[i] = F_[i];
170			log.T[i] = T_[i];
171			}
172			log.time = double(time1-time2)/10e9;
173			time2 = time1;
174			rt_pipe_write(&stream_pipe_,&log,sizeof(log), P_NORMAL);
175			#else
176			printf("Streaming is only supported on xenomai.\n");
177			exit(-1);
178			#endif
179			}
180			internal_going = going_;
181			rt_mutex_release(&mutex_);
182			}
183			//stop streaming
184			msg.command = 0x0000;
185			rt_dev_send(socket_, &msg, sizeof(msg), 0);
186			}
187
188			void AtiFTSensor::setBias()
189			{
190			double* force = NULL;
191			double* torque = NULL;
192			setBias(force, torque);
193			}
194
195			void AtiFTSensor::setBias(double* force, double* torque)
196			{
197			rt_mutex_acquire(&mutex_,TM_INFINITE);
198			for(int i=0; i<3; ++i) {
199			if (force == NULL) {
200			F_bias_[i] = F_[i];
201			} else {
202			F_bias_[i] = force[i];
203			}
204			if (torque == NULL) {
205			T_bias_[i] = T_[i];
206			} else {
207			T_bias_[i] = torque[i];
208			}
209			}
210			rt_mutex_release(&mutex_);
211			}
212
213			void AtiFTSensor::resetBias()
214			{
215			rt_mutex_acquire(&mutex_,TM_INFINITE);
216			for(int i=0; i<3; ++i)
217			{
218			F_bias_[i] = 0.;
219			T_bias_[i] = 0.;
220			}
221			rt_mutex_release(&mutex_);
222			}
223
224
225			void AtiFTSensor::getStatus(uint32_t& rdt_seq, uint32_t& ft_seq, uint32_t& status)
226			{
227			rt_mutex_acquire(&mutex_,TM_INFINITE);
228
229			rdt_seq = rdt_sequence_;
230			ft_seq = ft_sequence_;
231			status = status_;
232
233			rt_mutex_release(&mutex_);
234			}
235
236			void AtiFTSensor::getFT(double* force, double* torque)
237			{
238			rt_mutex_acquire(&mutex_,TM_INFINITE);
239
240			for(int i=0; i<3; ++i)
241			{
242			force[i] = F_[i];
243			torque[i] = T_[i];
244			}
245
246			rt_mutex_release(&mutex_);
247			}
248
249			void AtiFTSensor::stop()
250			{
251			if(initialized_)
252			{
253			rt_mutex_acquire(&mutex_,TM_INFINITE);
254			going_ = false;
255			rt_mutex_release(&mutex_);
256			reading_thread_.join();
257			rt_dev_close(socket_);
258			#ifdef XENOMAI
259			rt_pipe_delete(&stream_pipe_);
260			#endif
261			initialized_ = false;
262			}
263			}
264
265			void AtiFTSensor::stream(bool stream)
266			{
267			rt_mutex_acquire(&mutex_,TM_INFINITE);
268			streaming_ = stream;
269			rt_mutex_release(&mutex_);
270			}
271
272			AtiFTSensor::~AtiFTSensor()
273			{
274			stop();
275			}
276
277	✓✗✓✗	3	}