mirror of
https://gitlab.com/obbart/universal_robots_ros_driver.git
synced 2026-04-09 17:40:47 +02:00
Adds Safe Trajectory Follower implementation
Safe Trajectory Follower implements different approach for controlling the robot. Rather than calculate the interpolation steps in the driver and send the small interpolated steps over the network to the URScript program with 500Hz frequency, the coarser MoveIt trajectory is sent (with few Hz) and the interpolation steps are calculated by the URScript. The algorithm for time progress has also built-in protection against any delays induced by load on the driver, network or URControl - it will never "catch-up" dangerously when such delay are introduced, It will rather pause and wait for the next small interpolation step instructions and re-start the move slower - never skipping any interpolated steps. Those changes make Safe Trajectory Follower much more resilient to network communication problems and removes any superficial requirements for the network setup, kernel latency and no-load-requirement for the driver's PC - making it much more suitable for research, development and quick iteration loops. It works reliably even over WiFi.
This commit is contained in:
Jarek Potiuk
@@ -137,6 +137,8 @@ endif()
|
||||
add_compile_options(-Wall)
|
||||
add_compile_options(-Wextra)
|
||||
add_compile_options(-Wno-unused-parameter)
|
||||
add_compile_options(-Wno-ignored-qualifiers)
|
||||
add_compile_options(-Wno-return-type)
|
||||
|
||||
# support indigo's ros_control - This can be removed upon EOL indigo
|
||||
if("${controller_manager_msgs_VERSION}" VERSION_LESS "0.10.0")
|
||||
@@ -172,6 +174,7 @@ set(${PROJECT_NAME}_SOURCES
|
||||
src/ros/rt_publisher.cpp
|
||||
src/ros/service_stopper.cpp
|
||||
src/ros/trajectory_follower.cpp
|
||||
src/ros/safe_trajectory_follower.cpp
|
||||
src/ros/urscript_handler.cpp
|
||||
src/ur/stream.cpp
|
||||
src/ur/server.cpp
|
||||
|
||||
29
README.md
29
README.md
@@ -22,9 +22,9 @@ A new driver for the UR3/UR5/UR10 robot arms from Universal Robots. It is design
|
||||
|
||||
* Besides this, the driver subscribes to two new topics:
|
||||
|
||||
* */ur\_driver/URScript* : Takes messages of type _std\_msgs/String_ and directly forwards it to the robot. Note that no control is done on the input, so use at your own risk! Inteded for sending movel/movej commands directly to the robot, conveyor tracking and the like.
|
||||
* */ur\_driver/URScript* : Takes messages of type _std\_msgs/String_ and directly forwards it to the robot. Note that no control is done on the input, so use at your own risk! Intended for sending movel/movej commands directly to the robot, conveyor tracking and the like.
|
||||
|
||||
* */joint\_speed* : Takes messages of type _trajectory\_msgs/JointTrajectory_. Parses the first JointTracetoryPoint and sends the specified joint speeds and accelerations to the robot. This interface is intended for doing visual servoing and other kind of control that requires speed control rather than position control of the robot. Remember to set values for all 6 joints. Ignores the field joint\_names, so set the values in the correct order.
|
||||
* */joint\_speed* : Takes messages of type _trajectory\_msgs/JointTrajectory_. Parses the first JointTrajectoryPoint and sends the specified joint speeds and accelerations to the robot. This interface is intended for doing visual servoing and other kind of control that requires speed control rather than position control of the robot. Remember to set values for all 6 joints. Ignores the field joint\_names, so set the values in the correct order.
|
||||
|
||||
* Added support for ros_control.
|
||||
* As ros_control wants to have control over the robot at all times, ros_control compatibility is set via a parameter at launch-time.
|
||||
@@ -39,6 +39,31 @@ A new driver for the UR3/UR5/UR10 robot arms from Universal Robots. It is design
|
||||
|
||||
* Added activation modes: `Never`, `Always` and `OnStartup`. Sets wether a call to the `ur_driver/robot_enable` service is required at startup, at startup + on errors or never. Is intended as a safety feature to require some sort of manual intervention in case of driver crashes and robot safety faults.
|
||||
|
||||
* **Safe Trajectory Follower** mode of execution. In this mode the real-time control loop for the robot is shifted from the client PC running the driver to URscript executed on the URControl PC of the robot with the following features:
|
||||
* It works only with */follow\_joint\_trajectory* action for MoveIt integration. It is mutually exclusive with *ros_control*
|
||||
* It only implements "position + velocity" based control - it uses coarse positions and velocities calculated by MoveIt and performs cubic interpolation of joint positions (Bezier' curve) of positions. The positions are set using servoj command of URScript.
|
||||
* It is much more safe and resilient to connectivity problems than other methods of ur_modern_driver. It is resilient to additional load on client PC, latency of network and kernel of the PC the driver runs on. This makes it much better suitable for development/research quick iteration loops without separate dedicated physical setup.
|
||||
* Other methods of controlling the robot by ur_modern_driver are very fragile and require low-latency kernel, separated wired network and ideally no other network activity on the PC where ur_modern_driver runs.
|
||||
* **Safe Trajectory Follower** will never make dangerous "catch-up" when move is delayed. Other methods prioritise execution time of a move which sometimes might lead to situation where robot gets out of control and speeds up to catch-up - often resulting in Protective Stop. For Safe Trajectory Follower safety of the move has priority over move execution time.
|
||||
* The amount of TCP/IP communication between the driver and UR robot (URControl PC) is two orders of magnitude (around 100x) less with **Safe Trajectory Follower** than with other methods- coarse MoveIt generated trajectory is sent to robot and cubic interpolation of the move is calculated by the URScript program run on URControl PC.
|
||||
* Due to communication optimisations and **Safe Trajectory Follower** works reliably even over WiFi connection (!) which is impossible for other methods.
|
||||
* Time flow for the URScript program might be independent from "real time" and can be controlled to speed up or slow down execution of planned moves.
|
||||
* The Safe Trajectory Follower requires 3.0+ version firmware of the robot (servoj command must support lookahead_time and servoj_gain parameters)
|
||||
* There are several parameters that you can use to control Safe Trajectory Follower's behaviour:
|
||||
* **use_safe_trajectory_follower** - should be set to `true` to enable the follower
|
||||
* **time_interval** - time interval (in seconds) this is 'simulated' time for interpolation steps of the move. Together with *servoj_time* it can be used to change speed of moves even after they are planned by MoveIt. Default value is 0.008
|
||||
* **servoj_time** - time interval (real time) for which each interpolation step controls the robot (using servoj command). See below on examples of setting different time parameters. Default value is 0.008 (corresponds to expected 125Hz frequency of UR robot control)
|
||||
* **servoj_time_waiting** - time in seconds (real time) of internal active loop while the robot waits for new instructions in case of delays in communication. The smaller value, the faster robot restarts move after delay (but more stress is put on URControl processor). Default value is 0.001 (1000 Hz check frequency)
|
||||
* **max_waiting_time** - maximum time in seconds (real time) to wait for instructions from the drive before move is aborted. Defaults to 2 seconds.
|
||||
* **debug** - displays (visible in the log of Polyscope/pendant and URControl PC) helpful information about trajectory points messages exchanged Driver <-> URScript. It is safe to be run with real robot and introduces very small overhead on the execution of the moves
|
||||
* **more_debug** - displays even more debug information in the logs of Polyscope - details about interpolation loop calculations and execution. It adds a lot of overhead and is not recommended to be used with real robot (It's quite OK to run it with URSim though)
|
||||
* **servoj_gain** and **servoj_lookahead_time** - useful to control precision and speed of the position moves with servoj command (see URScript documentation for detailes)
|
||||
|
||||
Here are some examples of manipulating the time flow for **Safe Trajectory Follower** mode. You can use other settings but you should do it on your own risk.
|
||||
* Default mode: *servoj_time* = 0.008, *time_interval* = 0.008 : interpolation time flows with the same speed as real time - moves are executed as planned
|
||||
* Slow-motion mode: *servoj_time* = 0.008, *time_interval* = 0.004 : interpolation time flows 2x slower than real time, so the move is executed 2x slower than planned. Requires configuring MoveIt to accept much slower moves than expected (otherwise MoveIt cancels such move mid-way)
|
||||
* Fast-forward mode: *servoj_time* = 0.004, *time_interval* = 0.012 : interpolation time flows 3x faster than real time, so the move is 3x faster than planned. Might violate limits of the robot speed so use carefully and make sure you gradually increase the speed in-between to check how safe it is.
|
||||
|
||||
## Installation
|
||||
|
||||
**As the driver communicates with the robot via ethernet and depends on reliable continous communication, it is not possible to reliably control a UR from a virtual machine.**
|
||||
|
||||
@@ -39,7 +39,7 @@ private:
|
||||
std::condition_variable tj_cv_;
|
||||
std::thread tj_thread_;
|
||||
|
||||
TrajectoryFollower& follower_;
|
||||
TrajectoryFollowerInterface& follower_;
|
||||
|
||||
RobotState state_;
|
||||
std::array<double, 6> q_actual_, qd_actual_;
|
||||
@@ -61,7 +61,7 @@ private:
|
||||
bool updateState(RTShared& data);
|
||||
|
||||
public:
|
||||
ActionServer(TrajectoryFollower& follower, std::vector<std::string>& joint_names, double max_velocity);
|
||||
ActionServer(TrajectoryFollowerInterface& follower, std::vector<std::string>& joint_names, double max_velocity);
|
||||
|
||||
void start();
|
||||
virtual void onRobotStateChange(RobotState state);
|
||||
@@ -70,4 +70,4 @@ public:
|
||||
virtual bool consume(RTState_V1_8& state);
|
||||
virtual bool consume(RTState_V3_0__1& state);
|
||||
virtual bool consume(RTState_V3_2__3& state);
|
||||
};
|
||||
};
|
||||
|
||||
50
include/ur_modern_driver/ros/safe_trajectory_follower.h
Normal file
50
include/ur_modern_driver/ros/safe_trajectory_follower.h
Normal file
@@ -0,0 +1,50 @@
|
||||
#pragma once
|
||||
|
||||
#include <inttypes.h>
|
||||
#include <array>
|
||||
#include <atomic>
|
||||
#include <cstddef>
|
||||
#include <cstring>
|
||||
#include <string>
|
||||
#include <thread>
|
||||
#include <vector>
|
||||
#include "ur_modern_driver/log.h"
|
||||
#include "ur_modern_driver/ur/commander.h"
|
||||
#include "ur_modern_driver/ur/server.h"
|
||||
#include "ur_modern_driver/ros/trajectory_follower_interface.h"
|
||||
|
||||
class SafeTrajectoryFollower: public TrajectoryFollowerInterface
|
||||
{
|
||||
private:
|
||||
std::atomic<bool> running_;
|
||||
std::array<double, 6> last_positions_;
|
||||
URCommander &commander_;
|
||||
URServer server_;
|
||||
|
||||
double time_interval_, servoj_time_, servoj_time_waiting_, max_waiting_time_, \
|
||||
servoj_gain_, servoj_lookahead_time_;
|
||||
bool debug_, more_debug_;
|
||||
|
||||
std::string program_;
|
||||
|
||||
template <typename T>
|
||||
size_t append(uint8_t *buffer, T &val)
|
||||
{
|
||||
size_t s = sizeof(T);
|
||||
std::memcpy(buffer, &val, s);
|
||||
return s;
|
||||
}
|
||||
|
||||
bool execute(const std::array<double, 6> &positions,
|
||||
const std::array<double, 6> &velocities,
|
||||
double sample_number, double time_in_seconds);
|
||||
|
||||
public:
|
||||
SafeTrajectoryFollower(URCommander &commander, std::string &reverse_ip, int reverse_port, bool version_3);
|
||||
|
||||
bool start();
|
||||
bool execute(std::vector<TrajectoryPoint> &trajectory, std::atomic<bool> &interrupt);
|
||||
void stop();
|
||||
|
||||
virtual ~SafeTrajectoryFollower() {};
|
||||
};
|
||||
@@ -11,24 +11,9 @@
|
||||
#include "ur_modern_driver/log.h"
|
||||
#include "ur_modern_driver/ur/commander.h"
|
||||
#include "ur_modern_driver/ur/server.h"
|
||||
#include "ur_modern_driver/ros/trajectory_follower_interface.h"
|
||||
|
||||
struct TrajectoryPoint
|
||||
{
|
||||
std::array<double, 6> positions;
|
||||
std::array<double, 6> velocities;
|
||||
std::chrono::microseconds time_from_start;
|
||||
|
||||
TrajectoryPoint()
|
||||
{
|
||||
}
|
||||
|
||||
TrajectoryPoint(std::array<double, 6> &pos, std::array<double, 6> &vel, std::chrono::microseconds tfs)
|
||||
: positions(pos), velocities(vel), time_from_start(tfs)
|
||||
{
|
||||
}
|
||||
};
|
||||
|
||||
class TrajectoryFollower
|
||||
class TrajectoryFollower : public TrajectoryFollowerInterface
|
||||
{
|
||||
private:
|
||||
std::atomic<bool> running_;
|
||||
@@ -57,5 +42,6 @@ public:
|
||||
bool execute(std::array<double, 6> &positions);
|
||||
bool execute(std::vector<TrajectoryPoint> &trajectory, std::atomic<bool> &interrupt);
|
||||
void stop();
|
||||
void interrupt();
|
||||
};
|
||||
|
||||
virtual ~TrajectoryFollower() {};
|
||||
};
|
||||
|
||||
32
include/ur_modern_driver/ros/trajectory_follower_interface.h
Normal file
32
include/ur_modern_driver/ros/trajectory_follower_interface.h
Normal file
@@ -0,0 +1,32 @@
|
||||
#pragma once
|
||||
|
||||
#include <inttypes.h>
|
||||
#include <array>
|
||||
#include <atomic>
|
||||
#include <cstddef>
|
||||
#include <vector>
|
||||
|
||||
struct TrajectoryPoint
|
||||
{
|
||||
std::array<double, 6> positions;
|
||||
std::array<double, 6> velocities;
|
||||
std::chrono::microseconds time_from_start;
|
||||
|
||||
TrajectoryPoint()
|
||||
{
|
||||
}
|
||||
|
||||
TrajectoryPoint(std::array<double, 6> &pos, std::array<double, 6> &vel, std::chrono::microseconds tfs)
|
||||
: positions(pos), velocities(vel), time_from_start(tfs)
|
||||
{
|
||||
}
|
||||
};
|
||||
|
||||
class TrajectoryFollowerInterface
|
||||
{
|
||||
public:
|
||||
virtual bool start() = 0;
|
||||
virtual bool execute(std::vector<TrajectoryPoint> &trajectory, std::atomic<bool> &interrupt) = 0;
|
||||
virtual void stop() = 0;
|
||||
virtual ~TrajectoryFollowerInterface() {};
|
||||
};
|
||||
@@ -46,6 +46,7 @@ public:
|
||||
|
||||
std::string getIP();
|
||||
|
||||
bool read(char *character);
|
||||
bool read(uint8_t *buf, size_t buf_len, size_t &read);
|
||||
bool write(const uint8_t *buf, size_t buf_len, size_t &written);
|
||||
|
||||
|
||||
@@ -8,6 +8,8 @@
|
||||
#include <string>
|
||||
#include "ur_modern_driver/tcp_socket.h"
|
||||
|
||||
#define MAX_SERVER_BUF_LEN 50
|
||||
|
||||
class URServer : private TCPSocket
|
||||
{
|
||||
private:
|
||||
@@ -24,5 +26,6 @@ public:
|
||||
bool bind();
|
||||
bool accept();
|
||||
void disconnectClient();
|
||||
bool readLine(char* buffer, size_t buf_len);
|
||||
bool write(const uint8_t *buf, size_t buf_len, size_t &written);
|
||||
};
|
||||
};
|
||||
|
||||
@@ -2,7 +2,7 @@
|
||||
<!--
|
||||
Universal robot common bringup. Starts ur driver node and robot state
|
||||
publisher (translates joint positions to propper tfs).
|
||||
|
||||
|
||||
Usage:
|
||||
ur_common.launch robot_ip:=<value>
|
||||
-->
|
||||
@@ -12,14 +12,23 @@
|
||||
<arg name="min_payload" />
|
||||
<arg name="max_payload" />
|
||||
<arg name="prefix" default="" />
|
||||
<arg name="use_ros_control" default="false"/>
|
||||
<arg name="use_safe_trajectory_follower" default="false"/>
|
||||
<arg name="time_interval" default="0.008"/>
|
||||
<arg name="servoj_time" default="0.008" />
|
||||
<arg name="servoj_time_waiting" default="0.001" />
|
||||
<arg name="max_waiting_time" default="2.0" />
|
||||
<arg name="debug" default="false" />
|
||||
<arg name="more_debug" default="false" />
|
||||
<arg name="servoj_gain" default="100." />
|
||||
<arg name="servoj_lookahead_time" default="1." />
|
||||
<arg name="base_frame" default="$(arg prefix)base" />
|
||||
<arg name="tool_frame" default="$(arg prefix)tool0_controller" />
|
||||
<arg name="shutdown_on_disconnect" default="true" />
|
||||
|
||||
<!-- require_activation defines when the service /ur_driver/robot_enable needs to be called. -->
|
||||
<arg name="require_activation" default="Never" /> <!-- Never, Always, OnStartup -->
|
||||
|
||||
|
||||
<!-- The max_velocity parameter is only used for debugging in the ur_driver. It's not related to actual velocity limits -->
|
||||
<arg name="max_velocity" default="10.0"/> <!-- [rad/s] -->
|
||||
|
||||
@@ -30,10 +39,19 @@
|
||||
<!-- copy the specified IP address to be consistant with ROS-Industrial spec. -->
|
||||
<param name="prefix" type="str" value="$(arg prefix)" />
|
||||
<param name="robot_ip_address" type="str" value="$(arg robot_ip)" />
|
||||
<param name="use_ros_control" type="bool" value="$(arg use_ros_control)"/>
|
||||
<param name="use_safe_trajectory_follower" type="bool" value="$(arg use_safe_trajectory_follower)"/>
|
||||
<param name="min_payload" type="double" value="$(arg min_payload)" />
|
||||
<param name="max_payload" type="double" value="$(arg max_payload)" />
|
||||
<param name="max_velocity" type="double" value="$(arg max_velocity)" />
|
||||
<param name="time_interval" type="double" value="$(arg time_interval)" />
|
||||
<param name="servoj_time" type="double" value="$(arg servoj_time)" />
|
||||
<param name="servoj_time_waiting" type="double" value="$(arg servoj_time_waiting)" />
|
||||
<param name="max_waiting_time" type="double" value="$(arg max_waiting_time)" />
|
||||
<param name="debug" type="bool" value="$(arg debug)"/>
|
||||
<param name="more_debug" type="bool" value="$(arg more_debug)"/>
|
||||
<param name="servoj_gain" type="double" value="$(arg servoj_gain)" />
|
||||
<param name="servoj_lookahead_time" type="double" value="$(arg servoj_lookahead_time)" />
|
||||
<param name="base_frame" type="str" value="$(arg base_frame)"/>
|
||||
<param name="tool_frame" type="str" value="$(arg tool_frame)"/>
|
||||
<param name="require_activation" type="str" value="$(arg require_activation)" />
|
||||
|
||||
@@ -1,7 +1,7 @@
|
||||
#include "ur_modern_driver/ros/action_server.h"
|
||||
#include <cmath>
|
||||
|
||||
ActionServer::ActionServer(TrajectoryFollower& follower, std::vector<std::string>& joint_names, double max_velocity)
|
||||
ActionServer::ActionServer(TrajectoryFollowerInterface& follower, std::vector<std::string>& joint_names, double max_velocity)
|
||||
: as_(nh_, "follow_joint_trajectory", boost::bind(&ActionServer::onGoal, this, _1),
|
||||
boost::bind(&ActionServer::onCancel, this, _1), false)
|
||||
, joint_names_(joint_names)
|
||||
@@ -307,6 +307,7 @@ void ActionServer::trajectoryThread()
|
||||
|
||||
Result res;
|
||||
|
||||
LOG_INFO("Attempting to start follower %p", &follower_);
|
||||
if (follower_.start())
|
||||
{
|
||||
if (follower_.execute(trajectory, interrupt_traj_))
|
||||
|
||||
515
src/ros/safe_trajectory_follower.cpp
Normal file
515
src/ros/safe_trajectory_follower.cpp
Normal file
@@ -0,0 +1,515 @@
|
||||
#include "ur_modern_driver/ros/safe_trajectory_follower.h"
|
||||
#include <endian.h>
|
||||
#include <cmath>
|
||||
#include <ros/ros.h>
|
||||
|
||||
static const std::array<double, 6> EMPTY_VALUES = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
|
||||
|
||||
static const std::string TIME_INTERVAL("{{TIME_INTERVAL}}");
|
||||
static const std::string SERVOJ_TIME("{{SERVOJ_TIME}}");
|
||||
static const std::string SERVOJ_TIME_WAITING("{{SERVOJ_TIME_WAITING}}");
|
||||
static const std::string MAX_WAITING_TIME("{{MAX_WAITING_TIME}}");
|
||||
static const std::string DEBUG("{{DEBUG}}");
|
||||
static const std::string MORE_DEBUG("{{MORE_DEBUG}}");
|
||||
static const std::string SERVOJ_GAIN("{{SERVOJ_GAIN}}");
|
||||
static const std::string SERVOJ_LOOKAHEAD_TIME("{{SERVOJ_LOOKAHEAD_TIME}}");
|
||||
static const std::string REVERSE_IP("{{REVERSE_IP}}");
|
||||
static const std::string REVERSE_PORT("{{REVERSE_PORT}}");
|
||||
static const std::string POSITION_PROGRAM = R"(
|
||||
def driveRobotSafeTrajectory():
|
||||
|
||||
global JOINT_NUM = 6
|
||||
global TIME_INTERVAL = {{TIME_INTERVAL}}
|
||||
global SERVOJ_TIME = {{SERVOJ_TIME}}
|
||||
global SERVOJ_TIME_WAITING = {{SERVOJ_TIME_WAITING}}
|
||||
global MAX_WAITING_TIME = {{MAX_WAITING_TIME}}
|
||||
global DEBUG = {{DEBUG}}
|
||||
global MORE_DEBUG = {{MORE_DEBUG}}
|
||||
global EMPTY_VALUES = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
|
||||
global SERVOJ_GAIN = {{SERVOJ_GAIN}}
|
||||
global SERVOJ_LOOKAHEAD_TIME = {{SERVOJ_LOOKAHEAD_TIME}}
|
||||
global CONNECTION_NAME = "reverse_connection"
|
||||
global REVERSE_IP = "{{REVERSE_IP}}"
|
||||
global REVERSE_PORT = {{REVERSE_PORT}}
|
||||
|
||||
# NOTE All the global variables here are accessed by different threads
|
||||
# therefore they should be accessed within critical section. Those variables
|
||||
# are all prefixed with g_ . Whenever their values are needed they are copied
|
||||
# to similarly named l_ variable. Copying happens inside the critical section
|
||||
# and l_values might be used outside of it. This needs to be confirmed with UR
|
||||
# about the semantics of assignment operator (copying or by reference?).
|
||||
# Hopefully it's copying :).
|
||||
#
|
||||
# Please make sure to keep that pattern and do not access the global variables
|
||||
# outside of the critical section
|
||||
#
|
||||
# TO DO: Are those assignments by references or copies? We will find out soon
|
||||
# If not then assigning positions would not work either
|
||||
global g_position_previous = EMPTY_VALUES
|
||||
global g_position_target = EMPTY_VALUES
|
||||
global g_position_next = EMPTY_VALUES
|
||||
|
||||
global g_velocity_previous = EMPTY_VALUES
|
||||
global g_velocity_target = EMPTY_VALUES
|
||||
global g_velocity_next = EMPTY_VALUES
|
||||
|
||||
global g_time_previous = 0.0
|
||||
global g_time_target = 0.0
|
||||
global g_time_next = 0.0
|
||||
|
||||
global g_num_previous = -1
|
||||
global g_num_target = -1
|
||||
global g_num_next = -1
|
||||
|
||||
global g_received_waypoints_number = -1
|
||||
global g_requested_waypoints_number = -1
|
||||
|
||||
global g_total_elapsed_time = 0
|
||||
|
||||
global g_stopping = False
|
||||
|
||||
def send_message(message):
|
||||
socket_send_string(message, CONNECTION_NAME)
|
||||
socket_send_byte(10, CONNECTION_NAME)
|
||||
end
|
||||
|
||||
def is_waypoint_sentinel(waypoint):
|
||||
local l_previous_index = 2
|
||||
while l_previous_index < 1 + JOINT_NUM * 2 + 2: # Note - we do not check first two which are non-zero
|
||||
if waypoint[l_previous_index] != 0.0:
|
||||
return False
|
||||
end
|
||||
l_previous_index = l_previous_index + 1
|
||||
end
|
||||
return True
|
||||
end
|
||||
|
||||
def interpolate(time_within_segment, total_segment_time, start_pos, l_end_pos, l_start_vel, end_vel):
|
||||
local a = start_pos
|
||||
local b = l_start_vel
|
||||
local c = (-3 * a + 3 * l_end_pos - 2 * total_segment_time * b - total_segment_time * end_vel) / pow(total_segment_time, 2)
|
||||
local d = (2 * a - 2 * l_end_pos + total_segment_time * b + total_segment_time * end_vel) / pow(total_segment_time, 3)
|
||||
return a + b * time_within_segment + c * pow(time_within_segment, 2) + d * pow(time_within_segment, 3)
|
||||
end
|
||||
|
||||
def add_next_waypoint(waypoint):
|
||||
enter_critical
|
||||
|
||||
# Rotate the values received so far:
|
||||
# target -> previous
|
||||
g_position_previous = g_position_target
|
||||
g_velocity_previous = g_velocity_target
|
||||
g_time_previous = g_time_target
|
||||
g_num_previous = g_num_target
|
||||
|
||||
# next -> previous
|
||||
g_position_target = g_position_next
|
||||
g_velocity_target = g_velocity_next
|
||||
g_time_target = g_time_next
|
||||
g_num_target = g_num_next
|
||||
|
||||
# Decode the array received into next
|
||||
# waypoint 0 is number of entries in the received array
|
||||
g_num_next = waypoint[1]
|
||||
g_position_next = [waypoint[2], waypoint[3], waypoint[4], waypoint[5], waypoint[6], waypoint[7]]
|
||||
g_velocity_next = [waypoint[8], waypoint[9], waypoint[10], waypoint[11], waypoint[12], waypoint[13]]
|
||||
g_time_next = waypoint[14]
|
||||
|
||||
# store latest received waypoint number so that controlling thread knows it's been received already
|
||||
g_received_waypoints_number = g_num_next
|
||||
|
||||
if DEBUG:
|
||||
local l_received_waypoints_number = g_received_waypoints_number
|
||||
local l_waypoint = waypoint
|
||||
end
|
||||
|
||||
exit_critical
|
||||
|
||||
if DEBUG:
|
||||
textmsg("Received waypoint:")
|
||||
textmsg(l_received_waypoints_number)
|
||||
textmsg(l_waypoint)
|
||||
end
|
||||
end
|
||||
|
||||
# Thread controlling the motor. In the loop it checks first if it received the
|
||||
# requested waypoints and until it does, it syncs doing noting to the motor
|
||||
# once it received all up to requested waypoints it executes interpolation
|
||||
# between PREVIOUS AND TARGET points received and rquests the next waypoint request
|
||||
# to be sent.
|
||||
thread controllingThread():
|
||||
|
||||
local l_received_waypoints_number = -1
|
||||
local l_requested_waypoints_number = -1
|
||||
local l_stopped = False
|
||||
local l_current_position = get_actual_joint_positions()
|
||||
|
||||
enter_critical
|
||||
g_requested_waypoints_number = 2
|
||||
exit_critical
|
||||
|
||||
while True:
|
||||
|
||||
enter_critical
|
||||
l_requested_waypoints_number = g_requested_waypoints_number
|
||||
l_received_waypoints_number = g_received_waypoints_number
|
||||
exit_critical
|
||||
|
||||
local l_max_waiting_time_left = MAX_WAITING_TIME
|
||||
# if expected waypoint number not yet received wait so that receiving thread has time to receive it
|
||||
while l_received_waypoints_number < l_requested_waypoints_number and l_max_waiting_time_left > 0:
|
||||
|
||||
if DEBUG:
|
||||
textmsg("Waiting for the received waypoints number to catch up (received/requested):")
|
||||
textmsg(l_received_waypoints_number)
|
||||
textmsg(l_requested_waypoints_number)
|
||||
end
|
||||
# Keep robot in l_current position for short time and check if the next waipoint arrived
|
||||
servoj(l_current_position,t=SERVOJ_TIME_WAITING,lookahead_time=SERVOJ_LOOKAHEAD_TIME,gain=SERVOJ_GAIN)
|
||||
|
||||
enter_critical
|
||||
l_received_waypoints_number = g_received_waypoints_number
|
||||
exit_critical
|
||||
|
||||
l_max_waiting_time_left = l_max_waiting_time_left - SERVOJ_TIME_WAITING
|
||||
|
||||
end
|
||||
|
||||
if l_max_waiting_time_left <= 0:
|
||||
textmsg("Closing the connection on waiting too long.")
|
||||
socket_close(CONNECTION_NAME)
|
||||
halt
|
||||
end
|
||||
|
||||
# OK. We received next point, copy the required global variables into local ones
|
||||
enter_critical
|
||||
|
||||
local l_start_pos = g_position_previous
|
||||
local l_start_vel = g_velocity_previous
|
||||
local l_start_time = g_time_previous
|
||||
local l_start_num= g_num_previous
|
||||
local l_end_pos = g_position_target
|
||||
local l_end_vel = g_velocity_target
|
||||
local l_end_time = g_time_target
|
||||
local l_end_num = g_num_target
|
||||
local l_total_elapsed_time = g_total_elapsed_time
|
||||
|
||||
# Note we deliberately only read "stopping" state here and not update it below
|
||||
# so that stopping flag takes effect only after one additional interpolation loop is complete
|
||||
# and all points of the trajectory are processeed
|
||||
local l_stopping_after_next_interpolation = g_stopping
|
||||
|
||||
# And increasing the global requested number - informs sender thread that it needs to ask for it
|
||||
g_requested_waypoints_number = g_requested_waypoints_number + 1
|
||||
exit_critical
|
||||
|
||||
if DEBUG:
|
||||
textmsg("Starting interpolation. Segment from/to:")
|
||||
textmsg(l_start_num)
|
||||
textmsg(l_end_num)
|
||||
textmsg("Current time:")
|
||||
textmsg(l_total_elapsed_time)
|
||||
textmsg("Starting/Ending segment time:")
|
||||
textmsg(l_start_time)
|
||||
textmsg(l_end_time)
|
||||
end
|
||||
|
||||
l_current_position = l_start_pos
|
||||
|
||||
local l_total_segment_time = l_end_time - l_start_time
|
||||
|
||||
# Here perform the interpolation loop
|
||||
while l_total_elapsed_time <= l_end_time:
|
||||
if MORE_DEBUG:
|
||||
textmsg("Next step of interpolation:")
|
||||
end
|
||||
|
||||
local l_segment_elapsed_time = l_total_elapsed_time - l_start_time
|
||||
|
||||
# Calculate interpolation for all joints
|
||||
j = 0
|
||||
while j < JOINT_NUM:
|
||||
l_current_position[j] = interpolate(l_segment_elapsed_time, l_total_segment_time, l_start_pos[j], l_end_pos[j], l_start_vel[j], l_end_vel[j])
|
||||
j = j + 1
|
||||
end
|
||||
|
||||
if MORE_DEBUG:
|
||||
textmsg("Next step of interpolated position:")
|
||||
textmsg(l_current_position)
|
||||
textmsg("Current time:")
|
||||
textmsg(l_total_elapsed_time)
|
||||
textmsg("Running servoj command:")
|
||||
end
|
||||
|
||||
servoj(l_current_position,t=SERVOJ_TIME,lookahead_time=SERVOJ_LOOKAHEAD_TIME,gain=SERVOJ_GAIN)
|
||||
|
||||
enter_critical
|
||||
g_total_elapsed_time = g_total_elapsed_time + TIME_INTERVAL
|
||||
l_total_elapsed_time = g_total_elapsed_time
|
||||
exit_critical
|
||||
|
||||
if MORE_DEBUG:
|
||||
textmsg("Finishing interpolation step at time:")
|
||||
textmsg(l_total_elapsed_time)
|
||||
end
|
||||
end
|
||||
if DEBUG:
|
||||
textmsg("Ending interpolation segment at time:")
|
||||
textmsg(l_total_elapsed_time)
|
||||
end
|
||||
if l_stopping_after_next_interpolation:
|
||||
textmsg("Stopping the controlling thread on signal from receiving thread after one interpolation loop.")
|
||||
break
|
||||
end
|
||||
end
|
||||
textmsg("Ending controlling thread")
|
||||
end
|
||||
|
||||
# This thread sends requested waypoints number to the client when requested number is changed
|
||||
# It will send all the numbers from [already sent + 1, g_requested_waypoints_number] and waits
|
||||
# until requested waypoints number increases
|
||||
thread sendingThread():
|
||||
local controlling_thread = run controllingThread()
|
||||
local l_sent_waypoints_number = -1
|
||||
local l_requested_waypoints_number = -1
|
||||
local l_stopping = False
|
||||
|
||||
enter_critical
|
||||
l_requested_waypoints_number = g_requested_waypoints_number
|
||||
l_stopping = g_stopping
|
||||
exit_critical
|
||||
while not l_stopping:
|
||||
# wait until we have more requested waypoints to send than actually sent ones
|
||||
while l_sent_waypoints_number == l_requested_waypoints_number and not l_stopping:
|
||||
sleep(SERVOJ_TIME_WAITING)
|
||||
|
||||
enter_critical
|
||||
l_requested_waypoints_number = g_requested_waypoints_number
|
||||
l_stopping = g_stopping
|
||||
exit_critical
|
||||
|
||||
end
|
||||
if l_stopping:
|
||||
break
|
||||
end
|
||||
send_message(l_sent_waypoints_number + 1)
|
||||
l_sent_waypoints_number = l_sent_waypoints_number + 1
|
||||
if DEBUG:
|
||||
textmsg("Sent waypoint request number:")
|
||||
textmsg(l_sent_waypoints_number)
|
||||
end
|
||||
end
|
||||
textmsg("Joining controlling thread")
|
||||
join controlling_thread
|
||||
textmsg("Ending Sending thread")
|
||||
end
|
||||
|
||||
# Receiving thread - it will receive the next trajectory point over the TCP connection
|
||||
# It will increase the received waipoints_number on each request.
|
||||
thread receivingThread():
|
||||
local sending_thread = run sendingThread()
|
||||
while True:
|
||||
waypoint_received = socket_read_ascii_float(14, CONNECTION_NAME)
|
||||
if waypoint_received[0] == 0:
|
||||
# No new waypoint requested for the last 2 seconds
|
||||
textmsg("Not received trajectory for the last 2 seconds. Quitting")
|
||||
enter_critical
|
||||
g_stopping = True
|
||||
exit_critical
|
||||
|
||||
break
|
||||
elif waypoint_received[0] != JOINT_NUM * 2 + 2:
|
||||
textmsg("Received wrong number of floats in trajectory. This is certainly not OK.")
|
||||
textmsg(waypoint_received[0])
|
||||
|
||||
enter_critical
|
||||
g_stopping = True
|
||||
exit_critical
|
||||
|
||||
break
|
||||
elif is_waypoint_sentinel(waypoint_received):
|
||||
add_next_waypoint(waypoint_received)
|
||||
textmsg("Received sentinel waypoint. Finishing.")
|
||||
|
||||
enter_critical
|
||||
g_stopping = True
|
||||
g_received_waypoints_number = g_received_waypoints_number + 1
|
||||
exit_critical
|
||||
|
||||
break
|
||||
end
|
||||
add_next_waypoint(waypoint_received)
|
||||
end
|
||||
textmsg("Joining sendingThread")
|
||||
join sending_thread
|
||||
textmsg("Ending Receiving thread")
|
||||
end
|
||||
|
||||
textmsg("Opening socket")
|
||||
socket_open(REVERSE_IP, REVERSE_PORT, CONNECTION_NAME)
|
||||
textmsg("Socket opened")
|
||||
|
||||
receiving_thread = run receivingThread()
|
||||
textmsg("Joining receiving_thread")
|
||||
join receiving_thread
|
||||
textmsg("Closing reverse connection")
|
||||
socket_close(CONNECTION_NAME)
|
||||
textmsg("Exiting the program")
|
||||
end
|
||||
|
||||
)";
|
||||
|
||||
SafeTrajectoryFollower::SafeTrajectoryFollower(URCommander &commander, std::string &reverse_ip, int reverse_port,
|
||||
bool version_3)
|
||||
: running_(false)
|
||||
, commander_(commander)
|
||||
, server_(reverse_port)
|
||||
, time_interval_(0.008)
|
||||
, servoj_time_(0.008)
|
||||
, servoj_time_waiting_(0.001)
|
||||
, max_waiting_time_(2.0)
|
||||
, servoj_gain_(300.0)
|
||||
, servoj_lookahead_time_(0.03)
|
||||
, debug_(false)
|
||||
, more_debug_(false)
|
||||
{
|
||||
ros::param::get("~time_interval", time_interval_);
|
||||
ros::param::get("~servoj_time", servoj_time_);
|
||||
ros::param::get("~servoj_time_waiting", servoj_time_waiting_);
|
||||
ros::param::get("~max_waiting_time", max_waiting_time_);
|
||||
ros::param::get("~servoj_gain", servoj_gain_);
|
||||
ros::param::get("~servoj_lookahead_time", servoj_lookahead_time_);
|
||||
ros::param::get("~debug", debug_);
|
||||
ros::param::get("~more_debug", more_debug_);
|
||||
|
||||
std::string res(POSITION_PROGRAM);
|
||||
std::ostringstream out;
|
||||
if (!version_3) {
|
||||
LOG_ERROR("Safe Trajectory Follower only works for interface version > 3");
|
||||
std::exit(-1);
|
||||
}
|
||||
res.replace(res.find(TIME_INTERVAL), TIME_INTERVAL.length(), std::to_string(time_interval_));
|
||||
res.replace(res.find(SERVOJ_TIME_WAITING), SERVOJ_TIME_WAITING.length(), std::to_string(servoj_time_waiting_));
|
||||
res.replace(res.find(SERVOJ_TIME), SERVOJ_TIME.length(), std::to_string(servoj_time_));
|
||||
res.replace(res.find(MAX_WAITING_TIME), MAX_WAITING_TIME.length(), std::to_string(max_waiting_time_));
|
||||
res.replace(res.find(SERVOJ_GAIN), SERVOJ_GAIN.length(), std::to_string(servoj_gain_));
|
||||
res.replace(res.find(SERVOJ_LOOKAHEAD_TIME), SERVOJ_LOOKAHEAD_TIME.length(), std::to_string(servoj_lookahead_time_));
|
||||
res.replace(res.find(DEBUG), DEBUG.length(), debug_ ? "True" : "False");
|
||||
res.replace(res.find(MORE_DEBUG), MORE_DEBUG.length(), more_debug_ ? "True" : "False");
|
||||
res.replace(res.find(REVERSE_IP), REVERSE_IP.length(), reverse_ip);
|
||||
res.replace(res.find(REVERSE_PORT), REVERSE_PORT.length(), std::to_string(reverse_port));
|
||||
program_ = res;
|
||||
|
||||
if (!server_.bind())
|
||||
{
|
||||
LOG_ERROR("Failed to bind server, the port %d is likely already in use", reverse_port);
|
||||
std::exit(-1);
|
||||
}
|
||||
LOG_INFO("Safe Trajectory Follower is initialized!");
|
||||
}
|
||||
|
||||
bool SafeTrajectoryFollower::start()
|
||||
{
|
||||
LOG_INFO("Starting SafeTrajectoryFollower");
|
||||
|
||||
if (running_)
|
||||
return true; // not sure
|
||||
|
||||
LOG_INFO("Uploading trajectory program to robot");
|
||||
|
||||
if (!commander_.uploadProg(program_))
|
||||
{
|
||||
LOG_ERROR("Program upload failed!");
|
||||
return false;
|
||||
}
|
||||
|
||||
LOG_DEBUG("Awaiting incoming robot connection");
|
||||
|
||||
if (!server_.accept())
|
||||
{
|
||||
LOG_ERROR("Failed to accept incoming robot connection");
|
||||
return false;
|
||||
}
|
||||
|
||||
LOG_DEBUG("Robot successfully connected");
|
||||
return (running_ = true);
|
||||
}
|
||||
|
||||
bool SafeTrajectoryFollower::execute(const std::array<double, 6> &positions,
|
||||
const std::array<double, 6> &velocities,
|
||||
double sample_number, double time_in_seconds)
|
||||
{
|
||||
if (!running_)
|
||||
return false;
|
||||
|
||||
std::ostringstream out;
|
||||
|
||||
out << "(";
|
||||
out << sample_number << ",";
|
||||
for (auto const &pos: positions)
|
||||
{
|
||||
out << pos << ",";
|
||||
}
|
||||
for (auto const &vel: velocities)
|
||||
{
|
||||
out << vel << ",";
|
||||
}
|
||||
out << time_in_seconds << ")\r\n";
|
||||
|
||||
// I know it's ugly but it's the most efficient and fastest way
|
||||
// We have only ASCII characters and we can cast char -> uint_8
|
||||
const std::string tmp = out.str();
|
||||
const char *formatted_message = tmp.c_str();
|
||||
const uint8_t *buf = (uint8_t *) formatted_message;
|
||||
|
||||
size_t written;
|
||||
LOG_DEBUG("Sending message %s", formatted_message);
|
||||
|
||||
return server_.write(buf, strlen(formatted_message) + 1, written);
|
||||
}
|
||||
|
||||
bool SafeTrajectoryFollower::execute(std::vector<TrajectoryPoint> &trajectory, std::atomic<bool> &interrupt)
|
||||
{
|
||||
if (!running_)
|
||||
return false;
|
||||
|
||||
bool finished = false;
|
||||
|
||||
char* line[MAX_SERVER_BUF_LEN];
|
||||
|
||||
bool res = true;
|
||||
|
||||
while (!finished && !interrupt)
|
||||
{
|
||||
if (!server_.readLine((char *)line, MAX_SERVER_BUF_LEN))
|
||||
{
|
||||
LOG_DEBUG("Connection closed. Finishing!");
|
||||
finished = true;
|
||||
break;
|
||||
}
|
||||
unsigned int message_num=atoi((const char *) line);
|
||||
LOG_DEBUG("Received request %i", message_num);
|
||||
if (message_num < trajectory.size())
|
||||
{
|
||||
res = execute(trajectory[message_num].positions, trajectory[message_num].velocities,
|
||||
message_num, trajectory[message_num].time_from_start.count() / 1e6);
|
||||
} else
|
||||
{
|
||||
res = execute(EMPTY_VALUES, EMPTY_VALUES, message_num, 0.0);
|
||||
}
|
||||
if (!res)
|
||||
{
|
||||
finished = true;
|
||||
}
|
||||
}
|
||||
return res;
|
||||
}
|
||||
|
||||
void SafeTrajectoryFollower::stop()
|
||||
{
|
||||
if (!running_)
|
||||
return;
|
||||
|
||||
server_.disconnectClient();
|
||||
running_ = false;
|
||||
}
|
||||
@@ -13,6 +13,7 @@
|
||||
#include "ur_modern_driver/ros/rt_publisher.h"
|
||||
#include "ur_modern_driver/ros/service_stopper.h"
|
||||
#include "ur_modern_driver/ros/trajectory_follower.h"
|
||||
#include "ur_modern_driver/ros/safe_trajectory_follower.h"
|
||||
#include "ur_modern_driver/ros/urscript_handler.h"
|
||||
#include "ur_modern_driver/ur/commander.h"
|
||||
#include "ur_modern_driver/ur/factory.h"
|
||||
@@ -25,6 +26,7 @@
|
||||
static const std::string IP_ADDR_ARG("~robot_ip_address");
|
||||
static const std::string REVERSE_PORT_ARG("~reverse_port");
|
||||
static const std::string ROS_CONTROL_ARG("~use_ros_control");
|
||||
static const std::string SAFE_TRAJECTORY_FOLLOWER("~use_safe_trajectory_follower");
|
||||
static const std::string MAX_VEL_CHANGE_ARG("~max_vel_change");
|
||||
static const std::string PREFIX_ARG("~prefix");
|
||||
static const std::string BASE_FRAME_ARG("~base_frame");
|
||||
@@ -53,6 +55,7 @@ public:
|
||||
double max_vel_change;
|
||||
int32_t reverse_port;
|
||||
bool use_ros_control;
|
||||
bool use_safe_trajectory_follower;
|
||||
bool shutdown_on_disconnect;
|
||||
};
|
||||
|
||||
@@ -92,6 +95,7 @@ bool parse_args(ProgArgs &args)
|
||||
ros::param::param(MAX_VEL_CHANGE_ARG, args.max_vel_change, 15.0); // rad/s
|
||||
ros::param::param(MAX_VEL_CHANGE_ARG, args.max_velocity, 10.0);
|
||||
ros::param::param(ROS_CONTROL_ARG, args.use_ros_control, false);
|
||||
ros::param::param(SAFE_TRAJECTORY_FOLLOWER, args.use_safe_trajectory_follower, false);
|
||||
ros::param::param(PREFIX_ARG, args.prefix, std::string());
|
||||
ros::param::param(BASE_FRAME_ARG, args.base_frame, args.prefix + "base_link");
|
||||
ros::param::param(TOOL_FRAME_ARG, args.tool_frame, args.prefix + "tool0_controller");
|
||||
@@ -134,7 +138,20 @@ int main(int argc, char **argv)
|
||||
auto rt_commander = factory.getCommander(rt_stream);
|
||||
vector<IConsumer<RTPacket> *> rt_vec{ &rt_pub };
|
||||
|
||||
TrajectoryFollower traj_follower(*rt_commander, local_ip, args.reverse_port, factory.isVersion3());
|
||||
TrajectoryFollowerInterface *traj_follower(nullptr);
|
||||
|
||||
if (args.use_safe_trajectory_follower && !args.use_ros_control)
|
||||
{
|
||||
LOG_INFO("Use safe trajectory follower");
|
||||
traj_follower = new SafeTrajectoryFollower(*rt_commander, local_ip, args.reverse_port,factory.isVersion3());
|
||||
// We are leaking the follower here, but it's OK as this is once-a-lifetime event
|
||||
}
|
||||
else
|
||||
{
|
||||
LOG_INFO("Use standard trajectory follower");
|
||||
traj_follower = new TrajectoryFollower(*rt_commander, local_ip, args.reverse_port, factory.isVersion3());
|
||||
// We are leaking the follower here, but it's OK as this is once-a-lifetime event
|
||||
}
|
||||
|
||||
INotifier *notifier(nullptr);
|
||||
ROSController *controller(nullptr);
|
||||
@@ -142,14 +159,15 @@ int main(int argc, char **argv)
|
||||
if (args.use_ros_control)
|
||||
{
|
||||
LOG_INFO("ROS control enabled");
|
||||
controller = new ROSController(*rt_commander, traj_follower, args.joint_names, args.max_vel_change, args.tcp_link);
|
||||
// Note - we are sure that TrajectoryFollower is used here (see the args.use_ros_control above)
|
||||
controller = new ROSController(*rt_commander, *((TrajectoryFollower *) traj_follower), args.joint_names, args.max_vel_change, args.tcp_link);
|
||||
rt_vec.push_back(controller);
|
||||
services.push_back(controller);
|
||||
}
|
||||
else
|
||||
{
|
||||
LOG_INFO("ActionServer enabled");
|
||||
action_server = new ActionServer(traj_follower, args.joint_names, args.max_velocity);
|
||||
action_server = new ActionServer(*traj_follower, args.joint_names, args.max_velocity);
|
||||
rt_vec.push_back(action_server);
|
||||
services.push_back(action_server);
|
||||
}
|
||||
|
||||
@@ -111,6 +111,16 @@ std::string TCPSocket::getIP()
|
||||
return std::string(buf);
|
||||
}
|
||||
|
||||
bool TCPSocket::read(char *character)
|
||||
{
|
||||
size_t read_chars;
|
||||
// It's inefficient, but in our case we read very small messages
|
||||
// and the overhead connected with reading character by character is
|
||||
// negligible - adding buffering would complicate the code needlessly.
|
||||
return read((uint8_t *) character, 1, read_chars);
|
||||
}
|
||||
|
||||
|
||||
bool TCPSocket::read(uint8_t *buf, size_t buf_len, size_t &read)
|
||||
{
|
||||
read = 0;
|
||||
|
||||
@@ -86,3 +86,44 @@ bool URServer::write(const uint8_t* buf, size_t buf_len, size_t& written)
|
||||
{
|
||||
return client_.write(buf, buf_len, written);
|
||||
}
|
||||
|
||||
bool URServer::readLine(char* buffer, size_t buf_len)
|
||||
{
|
||||
char *current_pointer = buffer;
|
||||
char ch;
|
||||
size_t total_read;
|
||||
|
||||
if (buf_len <= 0 || buffer == NULL) {
|
||||
return false;
|
||||
}
|
||||
|
||||
total_read = 0;
|
||||
for (;;) {
|
||||
if (client_.read(&ch))
|
||||
{
|
||||
if (total_read < buf_len - 1) // just in case ...
|
||||
{
|
||||
total_read ++;
|
||||
*current_pointer++ = ch;
|
||||
}
|
||||
if (ch == '\n')
|
||||
{
|
||||
break;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
if (total_read == 0)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
else
|
||||
{
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
*current_pointer = '\0';
|
||||
return true;
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user