melfa_loopback_node.cpp
Go to the documentation of this file.
1 // Copyright 2018 Tokyo Opensource Robotics Kyokai Association (TORK)
2 //
3 // Licensed under the Apache License, Version 2.0 (the "License");
4 // you may not use this file except in compliance with the License.
5 // You may obtain a copy of the License at
6 //
7 // http://www.apache.org/licenses/LICENSE-2.0
8 //
9 // Unless required by applicable law or agreed to in writing, software
10 // distributed under the License is distributed on an "AS IS" BASIS,
11 // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12 // See the License for the specific language governing permissions and
13 // limitations under the License.
14 
20 #include <limits.h>
21 #include <pthread.h>
22 #include <sched.h>
23 #include <stdio.h>
24 #include <stdlib.h>
25 #include <sys/mman.h>
26 #include <string.h>
27 #include <sys/types.h>
28 #include <sys/socket.h>
29 #include <netinet/in.h>
30 #include <unistd.h>
31 #include <arpa/inet.h>
32 #include <ros/ros.h>
34 #include "melfa_driver/strdef.h"
35 
37 
39 {
40 private:
42  int counter_;
43 
45  double period_;
46 
47  int socket_;
48  struct sockaddr_in addr_;
49 
52 
55 
56 public:
57  LoopbackNode(double period)
58  : counter_(0), period_(period)
59  {
60  socket_ = socket(AF_INET, SOCK_DGRAM, 0);
61  addr_.sin_family = AF_INET;
62  addr_.sin_port = htons(10000);
63  addr_.sin_addr.s_addr = inet_addr ("127.0.0.1");
64 
65  bind(socket_, (struct sockaddr *)&addr_, sizeof(addr_));
66  }
68  {
69  close(socket_);
70  }
74  bool update(double timeout=0.06)
75  {
76  fd_set fds;
77  timeval time;
78 
79  // Record called time
80  time_old_ = time_now_;
81  time_now_ = ros::Time::now();
82 
83  FD_ZERO (&fds);
84  FD_SET (socket_, &fds);
85 
86  time.tv_sec = 0;
87  time.tv_usec = timeout * 1000;
88 
89  int r = select (socket_ + 1, &fds, NULL, NULL, &time);
90  if (r < 0)
91  {
92  ROS_ERROR ("select error: %s", strerror(errno));
93  ros::shutdown();
94  }
95  if (r > 0 && FD_ISSET (socket_, &fds))
96  {
97  struct sockaddr_in recv_addr;
98  socklen_t len;
99 
100  int size = recvfrom (socket_,
101  &recv_buff_, sizeof (recv_buff_),
102  0, (struct sockaddr *)&recv_addr, &len);
103  ROS_INFO("recv from: %s, size=%d", inet_ntoa(recv_addr.sin_addr), size);
104  counter_ ++;
105  // print recv packet
106  print_mxt_packet(recv_buff_);
107 
108  // loopback
109  send_buff_.Command = MXT_CMD_MOVE;
110  send_buff_.SendType = MXT_TYP_JOINT;
111  send_buff_.RecvType = MXT_TYP_JOINT;
112  send_buff_.RecvType1 = MXT_TYP_FJOINT;
113  send_buff_.RecvType2 = MXT_TYP_FB_JOINT;
114  send_buff_.RecvType3 = MXT_TYP_FBKCUR;
115 
116  send_buff_.dat.jnt.j1 = recv_buff_.dat.jnt.j1;
117  send_buff_.dat.jnt.j2 = recv_buff_.dat.jnt.j2;
118  send_buff_.dat.jnt.j3 = recv_buff_.dat.jnt.j3;
119  send_buff_.dat.jnt.j4 = recv_buff_.dat.jnt.j4;
120  send_buff_.dat.jnt.j5 = recv_buff_.dat.jnt.j5;
121  send_buff_.dat.jnt.j6 = recv_buff_.dat.jnt.j6;
122  send_buff_.dat.jnt.j7 = recv_buff_.dat.jnt.j7;
123  send_buff_.dat.jnt.j8 = recv_buff_.dat.jnt.j8;
124 
125  size = sendto (socket_,
126  (char *) &send_buff_, sizeof (send_buff_),
127  0, (struct sockaddr *) &recv_addr, sizeof (recv_addr));
128  if (size != sizeof (send_buff_))
129  {
130  ROS_WARN ("Failed to send packet.");
131  }
132  }
133  }
135  {
136  ROS_INFO("mxt.Command: %d", mxt.Command);
137  ROS_INFO("mxt.SendType: %d", mxt.SendType);
138  ROS_INFO("mxt.RecvType: %d", mxt.RecvType);
139  ROS_INFO("mxt.RecvType1: %d", mxt.RecvType1);
140  ROS_INFO("mxt.RecvType2: %d", mxt.RecvType2);
141  ROS_INFO("mxt.RecvType3: %d", mxt.RecvType3);
142  if (mxt.RecvType == MXT_TYP_JOINT)
143  {
144  ROS_INFO("mxt.dat.jnt: % 4.3f % 4.3f % 4.3f % 4.3f % 4.3f % 4.3f % 4.3f % 4.3f",
145  mxt.dat.jnt.j1,
146  mxt.dat.jnt.j2,
147  mxt.dat.jnt.j3,
148  mxt.dat.jnt.j4,
149  mxt.dat.jnt.j5,
150  mxt.dat.jnt.j6,
151  mxt.dat.jnt.j7,
152  mxt.dat.jnt.j8);
153  }
154  else
155  {
156  ROS_ERROR("Invalid mxt.RecvType");
157  }
158  }
160  {
161  stat.add ("Counter", counter_);
162 
163  double diff = (time_now_ - time_old_).toSec();
164  // Check over-run
165  if (diff > period_ * 1.2)
166  {
167  stat.summary(diagnostic_msgs::DiagnosticStatus::WARN,
168  "Periodic time exceeds 120%");
169  }
170  else
171  {
172  stat.summary(diagnostic_msgs::DiagnosticStatus::OK,
173  "Periodic time is normal");
174  }
175  stat.add ("Period", diff);
176  }
177 };
178 
180 {
181  double time_dif = (g_time_now - g_time_old).toSec();
182  stat.summary(diagnostic_msgs::DiagnosticStatus::OK, "Periodic time is normal");
183  stat.add ("Period", time_dif);
184 }
185 
189 int main (int argc, char **argv)
190 {
191  // init ROS node
192  ros::init (argc, argv, "melfa_loopback");
193  ros::NodeHandle nh;
194  // Parameters
195  bool realtime;
196  ros::param::param<bool>("~realtime", realtime, false);
197  double period;
198  ros::param::param<double>("~period", period, 0.0071);
199 
200  // Loopback node
201  LoopbackNode node(period);
202 
203  // Diagnostics
205  updater.setHardwareID("melfa_loopback");
206  updater.add("diagnose", &node, &LoopbackNode::diagnose);
207 
208  // Setup realtime scheduler
209  if (realtime)
210  {
211  struct sched_param param;
212  memset(&param, 0, sizeof(param));
213  int policy = SCHED_FIFO;
214  param.sched_priority = sched_get_priority_max(policy);
215 
216  ROS_WARN("Setting up Realtime scheduler");
217  if (sched_setscheduler(0, policy, &param) < 0) {
218  ROS_ERROR("sched_setscheduler: %s", strerror(errno));
219  ROS_ERROR("Please check you are using PREEMPT_RT kernel and set /etc/security/limits.conf");
220  exit (1);
221  }
222  if (mlockall(MCL_CURRENT|MCL_FUTURE) < 0)
223  {
224  ROS_ERROR("mlockall: %s", strerror(errno));
225  exit (1);
226  }
227  }
228  // Set spin rate frequency
229  ros::Rate rate (1.0 / period);
230  while (ros::ok ())
231  {
232  // Recieve and response
233  node.update();
234  // Update diagnostics
235  updater.update();
236  // Sleep for next cycle
237  rate.sleep();
238  }
239  return 0;
240 }
ros::Time g_time_old
#define MXT_TYP_FJOINT
Definition: strdef.h:95
float j3
Definition: strdef.h:30
uint16_t RecvType2
Definition: strdef.h:127
uint16_t RecvType1
Definition: strdef.h:120
void summary(unsigned char lvl, const std::string s)
void setHardwareID(const std::string &hwid)
float j1
Definition: strdef.h:28
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
IMETHOD Vector diff(const Vector &p_w_a, const Vector &p_w_b, double dt=1)
void add(const std::string &name, TaskFunction f)
uint16_t RecvType3
Definition: strdef.h:134
double period_
Periodic time [s].
float j6
Definition: strdef.h:33
updater
#define ROS_WARN(...)
ros::Time g_time_now
struct sockaddr_in addr_
void diagnose(diagnostic_updater::DiagnosticStatusWrapper &stat)
#define MXT_CMD_MOVE
Definition: strdef.h:78
void loop_diagnostic(diagnostic_updater::DiagnosticStatusWrapper &stat)
float j5
Definition: strdef.h:32
#define MXT_TYP_FBKCUR
Definition: strdef.h:103
uint16_t RecvType
Definition: strdef.h:81
union enet_rtcmd_str::rtdata dat
#define MXT_TYP_JOINT
Definition: strdef.h:90
#define ROS_INFO(...)
LoopbackNode(double period)
int main(int argc, char **argv)
Main function.
float j2
Definition: strdef.h:29
float j7
Definition: strdef.h:34
int counter_
Packet counter.
ROSCPP_DECL bool ok()
float j4
Definition: strdef.h:31
void print_mxt_packet(MXTCMD &mxt)
float j8
Definition: strdef.h:35
bool sleep()
bool update(double timeout=0.06)
#define MXT_TYP_FB_JOINT
Definition: strdef.h:98
uint16_t Command
Definition: strdef.h:76
uint16_t SendType
Definition: strdef.h:80
static Time now()
ROSCPP_DECL void shutdown()
void add(const std::string &key, const T &val)
#define ROS_ERROR(...)


melfa_driver
Author(s): Ryosuke Tajima
autogenerated on Mon Jun 10 2019 14:04:51