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

Clang-format run

Browse Source
This commit is contained in:
Simon Rasmussen
2017-07-09 02:54:49 +02:00
parent 577fcdbf98
commit 3a5fa23f6b
31 changed files with 343 additions and 343 deletions
Download Patch File Download Diff File Expand all files Collapse all files

128
src/ros/action_server.cpp
View File

@@ -1,14 +1,9 @@
#include <cmath>
#include "ur_modern_driver/ros/action_server.h"
#include <cmath>
ActionServer::ActionServer(TrajectoryFollower& 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
)
: as_(nh_, "follow_joint_trajectory", boost::bind(&ActionServer::onGoal, this, _1),
boost::bind(&ActionServer::onCancel, this, _1), false)
, joint_names_(joint_names)
, joint_set_(joint_names.begin(), joint_names.end())
, max_velocity_(max_velocity)
@@ -22,7 +17,7 @@ ActionServer::ActionServer(TrajectoryFollower& follower, std::vector<std::string
void ActionServer::start()
{
if(running_)
if (running_)
return;
LOG_INFO("Starting ActionServer");
@@ -35,23 +30,23 @@ void ActionServer::onRobotStateChange(RobotState state)
{
state_ = state;
//don't interrupt if everything is fine
if(state == RobotState::Running)
// don't interrupt if everything is fine
if (state == RobotState::Running)
return;
//don't retry interrupts
if(interrupt_traj_ || !has_goal_)
// don't retry interrupts
if (interrupt_traj_ || !has_goal_)
return;
//on successful lock we're not executing a goal so don't interrupt
if(tj_mutex_.try_lock())
// on successful lock we're not executing a goal so don't interrupt
if (tj_mutex_.try_lock())
{
tj_mutex_.unlock();
return;
}
interrupt_traj_ = true;
//wait for goal to be interrupted and automagically unlock when going out of scope
// wait for goal to be interrupted and automagically unlock when going out of scope
std::lock_guard<std::mutex> lock(tj_mutex_);
Result res;
@@ -60,7 +55,6 @@ void ActionServer::onRobotStateChange(RobotState state)
curr_gh_.setAborted(res, res.error_string);
}
bool ActionServer::updateState(RTShared& data)
{
q_actual_ = data.q_actual;
@@ -68,7 +62,6 @@ bool ActionServer::updateState(RTShared& data)
return true;
}
bool ActionServer::consume(RTState_V1_6__7& state)
{
return updateState(state);
@@ -86,7 +79,6 @@ bool ActionServer::consume(RTState_V3_2__3& state)
return updateState(state);
}
void ActionServer::onGoal(GoalHandle gh)
{
Result res;
@@ -94,7 +86,7 @@ void ActionServer::onGoal(GoalHandle gh)
LOG_INFO("Received new goal");
if(!validate(gh, res) || !try_execute(gh, res))
if (!validate(gh, res) || !try_execute(gh, res))
{
LOG_WARN("Goal error: %s", res.error_string.c_str());
gh.setRejected(res, res.error_string);
@@ -104,7 +96,7 @@ void ActionServer::onGoal(GoalHandle gh)
void ActionServer::onCancel(GoalHandle gh)
{
interrupt_traj_ = true;
//wait for goal to be interrupted
// wait for goal to be interrupted
std::lock_guard<std::mutex> lock(tj_mutex_);
Result res;
@@ -114,22 +106,22 @@ void ActionServer::onCancel(GoalHandle gh)
}
bool ActionServer::validate(GoalHandle& gh, Result& res)
{
{
return validateState(gh, res) && validateJoints(gh, res) && validateTrajectory(gh, res);
}
bool ActionServer::validateState(GoalHandle& gh, Result& res)
{
switch(state_)
switch (state_)
{
case RobotState::EmergencyStopped:
res.error_string = "Robot is emergency stopped";
return false;
case RobotState::ProtectiveStopped:
res.error_string = "Robot is protective stopped";
return false;
case RobotState::Error:
res.error_string = "Robot is not ready, check robot_mode";
return false;
@@ -149,7 +141,7 @@ bool ActionServer::validateJoints(GoalHandle& gh, Result& res)
auto const& joints = goal->trajectory.joint_names;
std::set<std::string> goal_joints(joints.begin(), joints.end());
if(goal_joints == joint_set_)
if (goal_joints == joint_set_)
return true;
res.error_code = Result::INVALID_JOINTS;
@@ -162,42 +154,42 @@ bool ActionServer::validateTrajectory(GoalHandle& gh, Result& res)
auto goal = gh.getGoal();
res.error_code = Result::INVALID_GOAL;
//must at least have one point
if(goal->trajectory.points.size() < 1)
// must at least have one point
if (goal->trajectory.points.size() < 1)
return false;
for(auto const& point : goal->trajectory.points)
for (auto const& point : goal->trajectory.points)
{
if(point.velocities.size() != joint_names_.size())
if (point.velocities.size() != joint_names_.size())
{
res.error_code = Result::INVALID_GOAL;
res.error_string = "Received a goal with an invalid number of velocities";
return false;
}
if(point.positions.size() != joint_names_.size())
if (point.positions.size() != joint_names_.size())
{
res.error_code = Result::INVALID_GOAL;
res.error_string = "Received a goal with an invalid number of positions";
return false;
}
for(auto const& velocity : point.velocities)
for (auto const& velocity : point.velocities)
{
if(!std::isfinite(velocity))
if (!std::isfinite(velocity))
{
res.error_string = "Received a goal with infinities or NaNs in velocity";
return false;
}
if(std::fabs(velocity) > max_velocity_)
if (std::fabs(velocity) > max_velocity_)
{
res.error_string = "Received a goal with velocities that are higher than " + std::to_string(max_velocity_);
return false;
}
}
for(auto const& position : point.positions)
for (auto const& position : point.positions)
{
if(!std::isfinite(position))
if (!std::isfinite(position))
{
res.error_string = "Received a goal with infinities or NaNs in positions";
return false;
@@ -205,34 +197,34 @@ bool ActionServer::validateTrajectory(GoalHandle& gh, Result& res)
}
}
//todo validate start position?
// todo validate start position?
return true;
}
inline std::chrono::microseconds convert(const ros::Duration &dur)
inline std::chrono::microseconds convert(const ros::Duration& dur)
{
return std::chrono::duration_cast<std::chrono::microseconds>(std::chrono::seconds(dur.sec) +
std::chrono::nanoseconds(dur.nsec));
std::chrono::nanoseconds(dur.nsec));
}
bool ActionServer::try_execute(GoalHandle& gh, Result& res)
{
if(!running_)
if (!running_)
{
res.error_string = "Internal error";
return false;
}
if(!tj_mutex_.try_lock())
if (!tj_mutex_.try_lock())
{
interrupt_traj_ = true;
res.error_string = "Received another trajectory";
curr_gh_.setAborted(res, res.error_string);
tj_mutex_.lock();
//todo: make configurable
// todo: make configurable
std::this_thread::sleep_for(std::chrono::milliseconds(250));
}
//locked here
// locked here
curr_gh_ = gh;
interrupt_traj_ = false;
has_goal_ = true;
@@ -244,12 +236,12 @@ bool ActionServer::try_execute(GoalHandle& gh, Result& res)
std::vector<size_t> ActionServer::reorderMap(std::vector<std::string> goal_joints)
{
std::vector<size_t> indecies;
for(auto const& aj : joint_names_)
for (auto const& aj : joint_names_)
{
size_t j = 0;
for(auto const& gj : goal_joints)
for (auto const& gj : goal_joints)
{
if(aj == gj)
if (aj == gj)
break;
j++;
}
@@ -261,40 +253,40 @@ std::vector<size_t> ActionServer::reorderMap(std::vector<std::string> goal_joint
void ActionServer::trajectoryThread()
{
LOG_INFO("Trajectory thread started");
while(running_)
while (running_)
{
std::unique_lock<std::mutex> lk(tj_mutex_);
if(!tj_cv_.wait_for(lk, std::chrono::milliseconds(100), [&]{return running_ && has_goal_;}))
if (!tj_cv_.wait_for(lk, std::chrono::milliseconds(100), [&] { return running_ && has_goal_; }))
continue;
LOG_INFO("Trajectory received and accepted");
curr_gh_.setAccepted();
auto goal = curr_gh_.getGoal();
std::vector<TrajectoryPoint> trajectory;
trajectory.reserve(goal->trajectory.points.size()+1);
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
// joint names of the goal might have a different ordering compared
// to what URScript expects so need to map between the two
auto mapping = reorderMap(goal->trajectory.joint_names);
LOG_INFO("Translating trajectory");
auto const& fp = goal->trajectory.points[0];
auto fpt = convert(fp.time_from_start);
//make sure we have a proper t0 position
if(fpt > std::chrono::microseconds(0))
// make sure we have a proper t0 position
if (fpt > std::chrono::microseconds(0))
{
LOG_INFO("Trajectory without t0 recieved, inserting t0 at currrent position");
trajectory.push_back(TrajectoryPoint(q_actual_, qd_actual_, std::chrono::microseconds(0)));
}
for(auto const& point : goal->trajectory.points)
for (auto const& point : goal->trajectory.points)
{
std::array<double, 6> pos, vel;
for(size_t i = 0; i < 6; i++)
for (size_t i = 0; i < 6; i++)
{
size_t idx = mapping[i];
pos[idx] = point.positions[i];
@@ -304,17 +296,19 @@ void ActionServer::trajectoryThread()
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();
double t =
std::chrono::duration_cast<std::chrono::duration<double>>(trajectory[trajectory.size() - 1].time_from_start)
.count();
LOG_INFO("Executing trajectory with %zu points and duration of %4.3fs", trajectory.size(), t);
Result res;
if(follower_.start())
if (follower_.start())
{
if(follower_.execute(trajectory, interrupt_traj_))
if (follower_.execute(trajectory, interrupt_traj_))
{
//interrupted goals must be handled by interrupt trigger
if(!interrupt_traj_)
// interrupted goals must be handled by interrupt trigger
if (!interrupt_traj_)
{
LOG_INFO("Trajectory executed successfully");
res.error_code = Result::SUCCESSFUL;
@@ -327,7 +321,7 @@ void ActionServer::trajectoryThread()
{
LOG_INFO("Trajectory failed");
res.error_code = -100;
res.error_string = "Connection to robot was lost";
res.error_string = "Connection to robot was lost";
curr_gh_.setAborted(res, res.error_string);
}
@@ -337,7 +331,7 @@ void ActionServer::trajectoryThread()
{
LOG_ERROR("Failed to start trajectory follower!");
res.error_code = -100;
res.error_string = "Robot connection could not be established";
res.error_string = "Robot connection could not be established";
curr_gh_.setAborted(res, res.error_string);
}