reverse_interface.cpp
Go to the documentation of this file.
1 // this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*-
2 
3 // -- BEGIN LICENSE BLOCK ----------------------------------------------
4 // Copyright 2021 FZI Forschungszentrum Informatik
5 // Created on behalf of Universal Robots A/S
6 //
7 // Licensed under the Apache License, Version 2.0 (the "License");
8 // you may not use this file except in compliance with the License.
9 // You may obtain a copy of the License at
10 //
11 // http://www.apache.org/licenses/LICENSE-2.0
12 //
13 // Unless required by applicable law or agreed to in writing, software
14 // distributed under the License is distributed on an "AS IS" BASIS,
15 // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
16 // See the License for the specific language governing permissions and
17 // limitations under the License.
18 // -- END LICENSE BLOCK ------------------------------------------------
19 
20 //----------------------------------------------------------------------
27 //----------------------------------------------------------------------
28 
30 #include <math.h>
31 
32 namespace urcl
33 {
34 namespace control
35 {
36 ReverseInterface::ReverseInterface(uint32_t port, std::function<void(bool)> handle_program_state,
37  std::chrono::milliseconds step_time)
38  : client_fd_(INVALID_SOCKET)
39  , server_(port)
40  , handle_program_state_(handle_program_state)
41  , step_time_(step_time)
42  , keep_alive_count_modified_deprecated_(false)
43 {
44  handle_program_state_(false);
45  server_.setMessageCallback(std::bind(&ReverseInterface::messageCallback, this, std::placeholders::_1,
46  std::placeholders::_2, std::placeholders::_3));
47  server_.setConnectCallback(std::bind(&ReverseInterface::connectionCallback, this, std::placeholders::_1));
48  server_.setDisconnectCallback(std::bind(&ReverseInterface::disconnectionCallback, this, std::placeholders::_1));
50  server_.start();
51 }
52 
53 bool ReverseInterface::write(const vector6d_t* positions, const comm::ControlMode control_mode,
54  const RobotReceiveTimeout& robot_receive_timeout)
55 {
56  const int message_length = 7;
58  {
59  return false;
60  }
61  uint8_t buffer[sizeof(int32_t) * MAX_MESSAGE_LENGTH];
62  uint8_t* b_pos = buffer;
63 
64  int read_timeout = 100;
65  // If control mode is stopped, we shouldn't verify robot receive timeout
66  if (control_mode != comm::ControlMode::MODE_STOPPED)
67  {
68  read_timeout = robot_receive_timeout.verifyRobotReceiveTimeout(control_mode, step_time_);
69  }
70 
71  // This can be removed once we remove the setkeepAliveCount() method
72  auto read_timeout_resolved = read_timeout;
74  {
75  // Translate keep alive count into read timeout. 20 milliseconds was the "old read timeout"
76  read_timeout_resolved = 20 * keepalive_count_;
77  }
78 
79  // The first element is always the read timeout.
80  int32_t val = read_timeout_resolved;
81  val = htobe32(val);
82  b_pos += append(b_pos, val);
83 
84  if (positions != nullptr)
85  {
86  for (auto const& pos : *positions)
87  {
88  int32_t val = static_cast<int32_t>(round(pos * MULT_JOINTSTATE));
89  val = htobe32(val);
90  b_pos += append(b_pos, val);
91  }
92  }
93  else
94  {
95  b_pos += 6 * sizeof(int32_t);
96  }
97 
98  // writing zeros to allow usage with other script commands
99  for (size_t i = message_length; i < MAX_MESSAGE_LENGTH - 1; i++)
100  {
101  val = htobe32(0);
102  b_pos += append(b_pos, val);
103  }
104 
105  val = htobe32(toUnderlying(control_mode));
106  b_pos += append(b_pos, val);
107 
108  size_t written;
109 
110  return server_.write(client_fd_, buffer, sizeof(buffer), written);
111 }
112 
114  const int point_number,
115  const RobotReceiveTimeout& robot_receive_timeout)
116 {
117  const int message_length = 3;
118  if (client_fd_ == INVALID_SOCKET)
119  {
120  return false;
121  }
122  uint8_t buffer[sizeof(int32_t) * MAX_MESSAGE_LENGTH];
123  uint8_t* b_pos = buffer;
124 
125  int read_timeout = robot_receive_timeout.verifyRobotReceiveTimeout(comm::ControlMode::MODE_FORWARD, step_time_);
126 
127  // This can be removed once we remove the setkeepAliveCount() method
128  auto read_timeout_resolved = read_timeout;
130  {
131  // Translate keep alive count into read timeout. 20 milliseconds was the "old read timeout"
132  read_timeout_resolved = 20 * keepalive_count_;
133  }
134 
135  // The first element is always the read timeout.
136  int32_t val = read_timeout_resolved;
137  val = htobe32(val);
138  b_pos += append(b_pos, val);
139 
140  val = htobe32(toUnderlying(trajectory_action));
141  b_pos += append(b_pos, val);
142 
143  val = htobe32(point_number);
144  b_pos += append(b_pos, val);
145 
146  // writing zeros to allow usage with other script commands
147  for (size_t i = message_length; i < MAX_MESSAGE_LENGTH - 1; i++)
148  {
149  val = htobe32(0);
150  b_pos += append(b_pos, val);
151  }
152 
154  b_pos += append(b_pos, val);
155 
156  size_t written;
157 
158  return server_.write(client_fd_, buffer, sizeof(buffer), written);
159 }
160 
162  const RobotReceiveTimeout& robot_receive_timeout)
163 {
164  const int message_length = 2;
165  if (client_fd_ == INVALID_SOCKET)
166  {
167  return false;
168  }
169  uint8_t buffer[sizeof(int32_t) * MAX_MESSAGE_LENGTH];
170  uint8_t* b_pos = buffer;
171 
172  int read_timeout = robot_receive_timeout.verifyRobotReceiveTimeout(comm::ControlMode::MODE_FREEDRIVE, step_time_);
173 
174  // This can be removed once we remove the setkeepAliveCount() method
175  auto read_timeout_resolved = read_timeout;
177  {
178  // Translate keep alive count into read timeout. 20 milliseconds was the "old read timeout"
179  read_timeout_resolved = 20 * keepalive_count_;
180  }
181 
182  // The first element is always the read timeout.
183  int32_t val = read_timeout_resolved;
184  val = htobe32(val);
185  b_pos += append(b_pos, val);
186 
187  val = htobe32(toUnderlying(freedrive_action));
188  b_pos += append(b_pos, val);
189 
190  // writing zeros to allow usage with other script commands
191  for (size_t i = message_length; i < MAX_MESSAGE_LENGTH - 1; i++)
192  {
193  val = htobe32(0);
194  b_pos += append(b_pos, val);
195  }
196 
198  b_pos += append(b_pos, val);
199 
200  size_t written;
201 
202  return server_.write(client_fd_, buffer, sizeof(buffer), written);
203 }
204 
205 void ReverseInterface::setKeepaliveCount(const uint32_t count)
206 {
207  URCL_LOG_WARN("DEPRECATION NOTICE: Setting the keepalive count has been deprecated. Instead you should set the "
208  "timeout directly in the write commands. Please change your code to set the read timeout in the write "
209  "commands "
210  "directly. This keepalive count will overwrite the timeout passed to the write functions.");
211  keepalive_count_ = count;
213 }
214 
216 {
217  if (client_fd_ == INVALID_SOCKET)
218  {
219  URCL_LOG_INFO("Robot connected to reverse interface. Ready to receive control commands.");
220  client_fd_ = filedescriptor;
221  handle_program_state_(true);
222  }
223  else
224  {
225  URCL_LOG_ERROR("Connection request to ReverseInterface received while connection already established. Only one "
226  "connection is allowed at a time. Ignoring this request.");
227  }
228 }
229 
231 {
232  URCL_LOG_INFO("Connection to reverse interface dropped.", filedescriptor);
234  handle_program_state_(false);
235 }
236 
237 void ReverseInterface::messageCallback(const socket_t filedescriptor, char* buffer, int nbytesrecv)
238 {
239  URCL_LOG_WARN("Message on ReverseInterface received. The reverse interface currently does not support any message "
240  "handling. This message will be ignored.");
241 }
242 
243 } // namespace control
244 } // namespace urcl
urcl::comm::ControlMode
ControlMode
Control modes as interpreted from the script runnning on the robot.
Definition: control_mode.h:42
socket_t
int socket_t
Definition: socket_t.h:57
urcl::comm::ControlMode::MODE_FORWARD
@ MODE_FORWARD
Set when trajectory forwarding is active.
urcl::control::ReverseInterface::disconnectionCallback
virtual void disconnectionCallback(const socket_t filedescriptor)
Definition: reverse_interface.cpp:230
urcl::RobotReceiveTimeout::verifyRobotReceiveTimeout
int verifyRobotReceiveTimeout(const comm::ControlMode control_mode, const std::chrono::milliseconds step_time) const
Helper function to verify that the robot receive timeout is configured appropriately given the curren...
Definition: robot_receive_timeout.cpp:64
INVALID_SOCKET
#define INVALID_SOCKET
Definition: socket_t.h:60
urcl::comm::TCPServer::start
void start()
Start event handling.
Definition: tcp_server.cpp:302
urcl::control::ReverseInterface::setKeepaliveCount
virtual void setKeepaliveCount(const uint32_t count)
Set the Keepalive count. This will set the number of allowed timeout reads on the robot.
Definition: reverse_interface.cpp:205
urcl::control::ReverseInterface::append
size_t append(uint8_t *buffer, T &val)
Definition: reverse_interface.h:171
urcl
Definition: bin_parser.h:36
urcl::control::ReverseInterface::MULT_JOINTSTATE
static const int32_t MULT_JOINTSTATE
Definition: reverse_interface.h:72
urcl::control::ReverseInterface::write
virtual bool write(const vector6d_t *positions, const comm::ControlMode control_mode=comm::ControlMode::MODE_IDLE, const RobotReceiveTimeout &robot_receive_timeout=RobotReceiveTimeout::millisec(20))
Writes needed information to the robot to be read by the URCaps program.
Definition: reverse_interface.cpp:53
URCL_LOG_ERROR
#define URCL_LOG_ERROR(...)
Definition: log.h:26
urcl::control::ReverseInterface::keepalive_count_
uint32_t keepalive_count_
Definition: reverse_interface.h:183
urcl::control::ReverseInterface::writeFreedriveControlMessage
bool writeFreedriveControlMessage(const FreedriveControlMessage freedrive_action, const RobotReceiveTimeout &robot_receive_timeout=RobotReceiveTimeout::millisec(200))
Writes needed information to the robot to be read by the URScript program.
Definition: reverse_interface.cpp:161
urcl::comm::ControlMode::MODE_FREEDRIVE
@ MODE_FREEDRIVE
Set when freedrive mode is active.
urcl::vector6d_t
std::array< double, 6 > vector6d_t
Definition: types.h:30
urcl::comm::TCPServer::setMaxClientsAllowed
void setMaxClientsAllowed(const uint32_t &max_clients_allowed)
Set the maximum number of clients allowed to connect to this server.
Definition: tcp_server.h:152
urcl::comm::TCPServer::setDisconnectCallback
void setDisconnectCallback(std::function< void(const socket_t)> func)
This callback will be triggered on clients disconnecting from the server.
Definition: tcp_server.h:92
urcl::control::ReverseInterface::connectionCallback
virtual void connectionCallback(const socket_t filedescriptor)
Definition: reverse_interface.cpp:215
urcl::control::TrajectoryControlMessage
TrajectoryControlMessage
Control messages for forwarding and aborting trajectories.
Definition: reverse_interface.h:48
urcl::control::ReverseInterface::messageCallback
virtual void messageCallback(const socket_t filedescriptor, char *buffer, int nbytesrecv)
Definition: reverse_interface.cpp:237
urcl::control::ReverseInterface::MAX_MESSAGE_LENGTH
static const int MAX_MESSAGE_LENGTH
Definition: reverse_interface.h:178
urcl::comm::TCPServer::setMessageCallback
void setMessageCallback(std::function< void(const socket_t, char *, int)> func)
This callback will be triggered on messages received on the socket.
Definition: tcp_server.h:103
urcl::control::ReverseInterface::step_time_
std::chrono::milliseconds step_time_
Definition: reverse_interface.h:181
urcl::control::ReverseInterface::client_fd_
socket_t client_fd_
Definition: reverse_interface.h:167
urcl::comm::TCPServer::write
bool write(const socket_t fd, const uint8_t *buf, const size_t buf_len, size_t &written)
Writes to a client.
Definition: tcp_server.cpp:309
urcl::control::ReverseInterface::keep_alive_count_modified_deprecated_
bool keep_alive_count_modified_deprecated_
Definition: reverse_interface.h:184
urcl::control::ReverseInterface::handle_program_state_
std::function< void(bool)> handle_program_state_
Definition: reverse_interface.h:180
URCL_LOG_INFO
#define URCL_LOG_INFO(...)
Definition: log.h:25
reverse_interface.h
urcl::comm::ControlMode::MODE_STOPPED
@ MODE_STOPPED
When this is set, the program is expected to stop and exit.
urcl::control::ReverseInterface::server_
comm::TCPServer server_
Definition: reverse_interface.h:168
urcl::control::ReverseInterface::writeTrajectoryControlMessage
bool writeTrajectoryControlMessage(const TrajectoryControlMessage trajectory_action, const int point_number=0, const RobotReceiveTimeout &robot_receive_timeout=RobotReceiveTimeout::millisec(200))
Writes needed information to the robot to be read by the URScript program.
Definition: reverse_interface.cpp:113
urcl::control::ReverseInterface::ReverseInterface
ReverseInterface()=delete
urcl::toUnderlying
constexpr std::underlying_type< E >::type toUnderlying(const E e) noexcept
Converts an enum type to its underlying type.
Definition: types.h:80
URCL_LOG_WARN
#define URCL_LOG_WARN(...)
Definition: log.h:24
urcl::RobotReceiveTimeout
RobotReceiveTimeout class containing a timeout configuration.
Definition: robot_receive_timeout.h:47
urcl::comm::TCPServer::setConnectCallback
void setConnectCallback(std::function< void(const socket_t)> func)
This callback will be triggered on clients connecting to the server.
Definition: tcp_server.h:81
urcl::control::FreedriveControlMessage
FreedriveControlMessage
Control messages for starting and stopping freedrive mode.
Definition: reverse_interface.h:58


ur_client_library
Author(s): Thomas Timm Andersen, Simon Rasmussen, Felix Exner, Lea Steffen, Tristan Schnell
autogenerated on Mon May 26 2025 02:35:58