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

WIP

Browse Source
This commit is contained in:
Simon Rasmussen
2017-07-07 15:26:39 +02:00
parent 0479c047d3
commit f35f40b45d
12 changed files with 114 additions and 71 deletions
Download Patch File Download Diff File Expand all files Collapse all files

54
src/ros/action_server.cpp
View File

@@ -51,12 +51,12 @@ void ActionServer::onRobotStateChange(RobotState state)
}
interrupt_traj_ = true;
//wait for goal to be interrupted
//wait for goal to be interrupted and automagically unlock when going out of scope
std::lock_guard<std::mutex> lock(tj_mutex_);
Result res;
res.error_code = -100;
res.error_string = "Received another trajectory";
res.error_string = "Robot safety stop";
curr_gh_.setAborted(res, res.error_string);
}
@@ -212,8 +212,8 @@ bool ActionServer::validateTrajectory(GoalHandle& gh, Result& res)
inline std::chrono::microseconds convert(const ros::Duration &dur)
{
return std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::seconds(dur.sec))
+ std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::nanoseconds(dur.nsec));
return std::chrono::duration_cast<std::chrono::microseconds>(std::chrono::seconds(dur.sec) +
std::chrono::nanoseconds(dur.nsec));
}
bool ActionServer::try_execute(GoalHandle& gh, Result& res)
@@ -261,7 +261,7 @@ std::vector<size_t> ActionServer::reorderMap(std::vector<std::string> goal_joint
void ActionServer::trajectoryThread()
{
LOG_INFO("Trajectory thread started");
follower_.start(); //todo check error
while(running_)
{
std::unique_lock<std::mutex> lk(tj_mutex_);
@@ -272,7 +272,8 @@ void ActionServer::trajectoryThread()
curr_gh_.setAccepted();
auto goal = curr_gh_.getGoal();
std::vector<TrajectoryPoint> trajectory(goal->trajectory.points.size());
std::vector<TrajectoryPoint> trajectory;
trajectory.reserve(goal->trajectory.points.size()+1);
//joint names of the goal might have a different ordering compared
//to what URScript expects so need to map between the two
@@ -281,7 +282,7 @@ void ActionServer::trajectoryThread()
LOG_INFO("Translating trajectory");
auto const& fp = goal->trajectory.points[0];
auto fpt = convert(fp.time_from_start)
auto fpt = convert(fp.time_from_start);
//make sure we have a proper t0 position
if(fpt > std::chrono::microseconds(0))
@@ -290,6 +291,7 @@ void ActionServer::trajectoryThread()
trajectory.push_back(TrajectoryPoint(q_actual_, qd_actual_, std::chrono::microseconds(0)));
}
int j = 0;
for(auto const& point : goal->trajectory.points)
{
std::array<double, 6> pos, vel;
@@ -299,35 +301,49 @@ void ActionServer::trajectoryThread()
pos[idx] = point.positions[i];
vel[idx] = point.velocities[i];
}
trajectory.push_back(TrajectoryPoint(pos, vel, convert(point.time_from_start)));
auto t = convert(point.time_from_start);
LOG_DEBUG("P[%d] = %ds, %dns -> %dus", j++, point.time_from_start.sec, point.time_from_start.nsec, t.count());
trajectory.push_back(TrajectoryPoint(pos, vel, t));
}
double t = std::chrono::duration_cast<std::chrono::duration<double>>(trajectory[trajectory.size()-1].time_from_start).count();
LOG_INFO("Trajectory with %d points and duration of %f constructed, executing now", trajectory.size(), t);
LOG_INFO("Executing trajectory with %d points and duration of %4.3fs", trajectory.size(), t);
Result res;
if(follower_.execute(trajectory, interrupt_traj_))
if(follower_.start())
{
//interrupted goals must be handled by interrupt trigger
if(!interrupt_traj_)
if(follower_.execute(trajectory, interrupt_traj_))
{
LOG_INFO("Trajectory executed successfully");
res.error_code = Result::SUCCESSFUL;
curr_gh_.setSucceeded(res);
//interrupted goals must be handled by interrupt trigger
if(!interrupt_traj_)
{
LOG_INFO("Trajectory executed successfully");
res.error_code = Result::SUCCESSFUL;
curr_gh_.setSucceeded(res);
}
else
LOG_INFO("Trajectory interrupted");
}
else
LOG_INFO("Trajectory interrupted");
{
LOG_INFO("Trajectory failed");
res.error_code = -100;
res.error_string = "Connection to robot was lost";
curr_gh_.setAborted(res, res.error_string);
}
follower_.stop();
}
else
{
LOG_INFO("Trajectory failed");
LOG_ERROR("Failed to start trajectory follower!");
res.error_code = -100;
res.error_string = "Connection to robot was lost";
res.error_string = "Robot connection could not be established";
curr_gh_.setAborted(res, res.error_string);
}
has_goal_ = false;
lk.unlock();
}
follower_.stop();
}

50
src/ros/trajectory_follower.cpp
View File

@@ -65,7 +65,7 @@ def driverProg():
end
)";
TrajectoryFollower::TrajectoryFollower(URCommander &commander, int reverse_port, bool version_3)
TrajectoryFollower::TrajectoryFollower(URCommander &commander, std::string& reverse_ip, int reverse_port, bool version_3)
: running_(false)
, commander_(commander)
, reverse_port_(reverse_port)
@@ -75,7 +75,6 @@ TrajectoryFollower::TrajectoryFollower(URCommander &commander, int reverse_port,
, server_(reverse_port)
{
std::string res(POSITION_PROGRAM);
res.replace(res.find(JOINT_STATE_REPLACE), JOINT_STATE_REPLACE.length(), std::to_string(MULT_JOINTSTATE_));
std::ostringstream out;
@@ -85,17 +84,16 @@ TrajectoryFollower::TrajectoryFollower(URCommander &commander, int reverse_port,
res.replace(res.find(SERVO_J_REPLACE), SERVO_J_REPLACE.length(), out.str());
program_ = res;
}
if(!server_.bind())
{
LOG_ERROR("Failed to bind server, the port %d is likely already in use", reverse_port);
std::exit(-1);
}
std::string TrajectoryFollower::buildProgram()
{
std::string res(program_);
std::string IP(server_.getIP());
LOG_INFO("Local IP: %s ", IP.c_str());
res.replace(res.find(SERVER_IP_REPLACE), SERVER_IP_REPLACE.length(), "127.0.0.1");
res.replace(res.find(SERVER_IP_REPLACE), SERVER_IP_REPLACE.length(), reverse_ip);
res.replace(res.find(SERVER_PORT_REPLACE), SERVER_PORT_REPLACE.length(), std::to_string(reverse_port_));
return res;
program_ = res;
}
bool TrajectoryFollower::start()
@@ -103,23 +101,15 @@ bool TrajectoryFollower::start()
if(running_)
return true; //not sure
if(!server_.bind())
{
LOG_ERROR("Failed to bind server");
return false;
}
LOG_INFO("Uploading trajectory program to robot");
std::string prog(buildProgram());
//std::string prog = "socket_open(\"127.0.0.1\", 50001)\n";
if(!commander_.uploadProg(prog))
if(!commander_.uploadProg(program_))
{
LOG_ERROR("Program upload failed!");
return false;
}
LOG_INFO("Awaiting incomming robot connection");
LOG_DEBUG("Awaiting incomming robot connection");
if(!server_.accept())
{
@@ -127,7 +117,7 @@ bool TrajectoryFollower::start()
return false;
}
LOG_INFO("Robot successfully connected");
LOG_DEBUG("Robot successfully connected");
return (running_ = true);
}
@@ -182,7 +172,7 @@ bool TrajectoryFollower::execute(std::vector<TrajectoryPoint> &trajectory, std::
typedef high_resolution_clock Clock;
typedef Clock::time_point Time;
auto const& last = trajectory[trajectory.size()-1];
auto& last = trajectory[trajectory.size()-1];
auto& prev = trajectory[0];
Time t0 = Clock::now();
@@ -202,8 +192,6 @@ bool TrajectoryFollower::execute(std::vector<TrajectoryPoint> &trajectory, std::
auto duration = point.time_from_start - prev.time_from_start;
double d_s = duration_cast<double_seconds>(duration).count();
LOG_INFO("Point duration: %f", d_s);
//interpolation loop
while(!interrupt)
{
@@ -243,13 +231,7 @@ bool TrajectoryFollower::execute(std::vector<TrajectoryPoint> &trajectory, std::
//the interpolation loop above but rather some position between
//t[N-1] and t[N] where N is the number of trajectory points.
//To make sure this does not happen the last position is sent
//if not interrupted.
if(!interrupt)
{
return execute(last.positions, true);
}
return true;
return execute(last.positions, true);
}
void TrajectoryFollower::stop()
@@ -260,6 +242,6 @@ void TrajectoryFollower::stop()
std::array<double, 6> empty;
execute(empty, false);
//server_.disconnect();
server_.disconnectClient();
running_ = false;
}