Obbart/universal_robots_ros_driver
1
0
Fork 0
mirror of https://gitlab.com/obbart/universal_robots_ros_driver.git synced 2026-04-12 11:00:47 +02:00
Code Releases Activity

Merge pull request #9 from NoMagicAi/SAFE_TRAJECTORY_FOLLOWER

Browse Source 
Adds Low Bandwith Trajectory Follower implementation
This commit is contained in:
G.A. vd. Hoorn
2018-09-27 16:06:39 +02:00
committed by GitHub
parent a2f5f4bed5 c72ec5bded
commit 50bd24dd3b
23 changed files with 791 additions and 60 deletions
Download Patch File Download Diff File Expand all files Collapse all files

3
src/ros/action_server.cpp
View File

@@ -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(ActionTrajectoryFollowerInterface& 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_))

386
src/ros/lowbandwidth_trajectory_follower.cpp Normal file
View File

@@ -0,0 +1,386 @@
#include "ur_modern_driver/ros/lowbandwidth_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 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 MAX_JOINT_DIFFERENCE("{{MAX_JOINT_DIFFERENCE}}");
static const std::string POSITION_PROGRAM = R"(
def driveRobotLowBandwidthTrajectory():
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 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}}
global MAX_JOINT_DIFFERENCE = {{MAX_JOINT_DIFFERENCE}}
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:
if waypoint[l_previous_index] != 0.0:
return False
end
l_previous_index = l_previous_index + 1
end
return True
end
def is_final_position_reached(position):
local l_current_position = get_actual_joint_positions()
local l_index = 0
while l_index < JOINT_NUM:
if norm(position[l_index] - l_current_position[l_index]) > MAX_JOINT_DIFFERENCE:
return False
end
l_index = l_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
g_position_previous = g_position_target
g_velocity_previous = g_velocity_target
g_time_previous = g_time_target
g_num_previous = g_num_target
g_position_target = g_position_next
g_velocity_target = g_velocity_next
g_time_target = g_time_next
g_num_target = g_num_next
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]
g_received_waypoints_number = g_num_next
exit_critical
end
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
exit_critical
local l_max_waiting_time_left = MAX_WAITING_TIME
while l_received_waypoints_number < l_requested_waypoints_number and l_max_waiting_time_left > 0:
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
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
local l_stopping_after_next_interpolation = g_stopping
g_requested_waypoints_number = g_requested_waypoints_number + 1
exit_critical
l_current_position = l_start_pos
local l_total_segment_time = l_end_time - l_start_time
while l_total_elapsed_time <= l_end_time:
local l_segment_elapsed_time = l_total_elapsed_time - l_start_time
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
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
end
if l_stopping_after_next_interpolation:
while not is_final_position_reached(l_end_pos):
textmsg("Catching up on final position not reached first time.")
servoj(l_end_pos,t=SERVOJ_TIME,lookahead_time=SERVOJ_LOOKAHEAD_TIME,gain=SERVOJ_GAIN)
end
textmsg("Finishing the controlling thread. Final position reached.")
break
end
end
end
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:
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
end
join controlling_thread
end
thread receivingThread():
local sending_thread = run sendingThread()
while True:
waypoint_received = socket_read_ascii_float(14, CONNECTION_NAME)
if waypoint_received[0] == 0:
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)
enter_critical
g_stopping = True
g_received_waypoints_number = g_received_waypoints_number + 1
exit_critical
break
end
add_next_waypoint(waypoint_received)
end
join sending_thread
end
socket_open(REVERSE_IP, REVERSE_PORT, CONNECTION_NAME)
receiving_thread = run receivingThread()
join receiving_thread
socket_close(CONNECTION_NAME)
textmsg("Exiting the program")
end
)";
LowBandwidthTrajectoryFollower::LowBandwidthTrajectoryFollower(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)
, max_joint_difference_(0.01)
{
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("~max_joint_difference", max_joint_difference_);
std::string res(POSITION_PROGRAM);
std::ostringstream out;
if (!version_3) {
LOG_ERROR("Low Bandwidth 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(REVERSE_IP), REVERSE_IP.length(), reverse_ip);
res.replace(res.find(REVERSE_PORT), REVERSE_PORT.length(), std::to_string(reverse_port));
res.replace(res.find(MAX_JOINT_DIFFERENCE), MAX_JOINT_DIFFERENCE.length(), std::to_string(max_joint_difference_));
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("Low Bandwidth Trajectory Follower is initialized!");
}
bool LowBandwidthTrajectoryFollower::start()
{
LOG_INFO("Starting LowBandwidthTrajectoryFollower");
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 LowBandwidthTrajectoryFollower::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 LowBandwidthTrajectoryFollower::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 LowBandwidthTrajectoryFollower::stop()
{
if (!running_)
return;
server_.disconnectClient();
running_ = false;
}

24
src/ros_main.cpp
View File

@@ -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/lowbandwidth_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 LOW_BANDWIDTH_TRAJECTORY_FOLLOWER("~use_lowbandwidth_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_lowbandwidth_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(LOW_BANDWIDTH_TRAJECTORY_FOLLOWER, args.use_lowbandwidth_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,22 +138,34 @@ 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());
INotifier *notifier(nullptr);
ROSController *controller(nullptr);
ActionServer *action_server(nullptr);
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);
TrajectoryFollower *traj_follower = new TrajectoryFollower(
*rt_commander, local_ip, args.reverse_port, factory.isVersion3());
controller = new ROSController(*rt_commander, *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);
ActionTrajectoryFollowerInterface *traj_follower(nullptr);
if (args.use_lowbandwidth_trajectory_follower)
{
LOG_INFO("Use low bandwidth trajectory follower");
traj_follower = new LowBandwidthTrajectoryFollower(*rt_commander,
local_ip, args.reverse_port,factory.isVersion3());
}
else
{
LOG_INFO("Use standard trajectory follower");
traj_follower = new TrajectoryFollower(*rt_commander, local_ip, args.reverse_port, factory.isVersion3());
}
action_server = new ActionServer(*traj_follower, args.joint_names, args.max_velocity);
rt_vec.push_back(action_server);
services.push_back(action_server);
}

14
src/tcp_socket.cpp
View File

@@ -90,7 +90,7 @@ void TCPSocket::close()
if (state_ != SocketState::Connected)
return;
state_ = SocketState::Closed;
::shutdown(socket_fd_, SHUT_RDWR);
::close(socket_fd_);
socket_fd_ = -1;
}
@@ -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;
@@ -154,4 +164,4 @@ bool TCPSocket::write(const uint8_t *buf, size_t buf_len, size_t &written)
}
return true;
}
}

41
src/ur/server.cpp
View File

@@ -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;
}