26 : as_(nh_,
"follow_joint_trajectory",
boost::bind(&
ActionServer::onGoal, this, _1),
28 , joint_names_(joint_names)
29 , joint_set_(joint_names.begin(), joint_names.end())
30 , max_velocity_(max_velocity)
31 , interrupt_traj_(false)
71 std::lock_guard<std::mutex> lock(
tj_mutex_);
74 res.error_code = -100;
75 res.error_string =
"Robot safety stop";
106 res.error_code = -100;
112 LOG_WARN(
"Goal error: %s", res.error_string.c_str());
121 std::lock_guard<std::mutex> lock(
tj_mutex_);
124 res.error_code = -100;
125 res.error_string =
"Goal cancelled by client";
139 res.error_string =
"Robot is emergency stopped";
143 res.error_string =
"Robot is protective stopped";
147 res.error_string =
"Robot is not ready, check robot_mode";
154 res.error_string =
"Undefined state";
162 auto const& joints = goal->trajectory.joint_names;
163 std::set<std::string> goal_joints(joints.begin(), joints.end());
168 res.error_code = Result::INVALID_JOINTS;
169 res.error_string =
"Invalid joint names for goal\n";
170 res.error_string +=
"Expected: ";
171 std::for_each(goal_joints.begin(), goal_joints.end(),
172 [&res](std::string joint) { res.error_string += joint +
", "; });
173 res.error_string +=
"\nFound: ";
174 std::for_each(
joint_set_.begin(),
joint_set_.end(), [&res](std::string joint) { res.error_string += joint +
", "; });
181 res.error_code = Result::INVALID_GOAL;
184 if (goal->trajectory.points.size() < 1)
187 for (
auto const& point : goal->trajectory.points)
191 res.error_code = Result::INVALID_GOAL;
192 res.error_string =
"Received a goal with an invalid number of velocities";
198 res.error_code = Result::INVALID_GOAL;
199 res.error_string =
"Received a goal with an invalid number of positions";
203 for (
auto const& velocity : point.velocities)
205 if (!std::isfinite(velocity))
207 res.error_string =
"Received a goal with infinities or NaNs in velocity";
213 "Received a goal with velocities that are higher than max_velocity_ " + std::to_string(
max_velocity_);
217 for (
auto const& position : point.positions)
219 if (!std::isfinite(position))
221 res.error_string =
"Received a goal with infinities or NaNs in positions";
234 return std::chrono::duration_cast<std::chrono::microseconds>(std::chrono::seconds(dur.
sec) +
235 std::chrono::nanoseconds(dur.
nsec));
242 res.error_string =
"Internal error";
248 res.error_string =
"Received another trajectory";
252 std::this_thread::sleep_for(std::chrono::milliseconds(250));
265 std::vector<size_t> indecies;
269 for (
auto const& gj : goal_joints)
275 indecies.push_back(j);
282 LOG_INFO(
"Trajectory thread started");
286 std::unique_lock<std::mutex> lk(
tj_mutex_);
287 if (!
tj_cv_.wait_for(lk, std::chrono::milliseconds(100), [&] { return running_ && has_goal_; }))
290 LOG_INFO(
"Trajectory received and accepted");
294 std::vector<TrajectoryPoint> trajectory;
295 trajectory.reserve(goal->trajectory.points.size() + 1);
303 auto const& fp = goal->trajectory.points[0];
304 auto fpt =
convert(fp.time_from_start);
307 if (fpt > std::chrono::microseconds(0))
309 LOG_INFO(
"Trajectory without t0 recieved, inserting t0 at currrent position");
313 for (
auto const& point : goal->trajectory.points)
315 std::array<double, 6> pos, vel;
316 for (
size_t i = 0; i < 6; i++)
319 pos[idx] = point.positions[i];
320 vel[idx] = point.velocities[i];
322 auto t =
convert(point.time_from_start);
327 std::chrono::duration_cast<std::chrono::duration<double>>(trajectory[trajectory.size() - 1].time_from_start)
329 LOG_INFO(
"Executing trajectory with %zu points and duration of %4.3fs", trajectory.size(), t);
341 LOG_INFO(
"Trajectory executed successfully");
342 res.error_code = Result::SUCCESSFUL;
351 res.error_code = -100;
352 res.error_string =
"Connection to robot was lost";
360 LOG_ERROR(
"Failed to start trajectory follower!");
361 res.error_code = -100;
362 res.error_string =
"Robot connection could not be established";
virtual bool execute(std::vector< TrajectoryPoint > &trajectory, std::atomic< bool > &interrupt)=0
std::condition_variable tj_cv_
bool validateState(GoalHandle &gh, Result &res)
bool validateJoints(GoalHandle &gh, Result &res)
control_msgs::FollowJointTrajectoryResult Result
std::atomic< bool > running_
virtual bool consume(RTState_V1_6__7 &state)
void setRejected(const Result &result=Result(), const std::string &text=std::string(""))
std::atomic< bool > has_goal_
boost::shared_ptr< const Goal > getGoal() const
std::array< double, 6 > qd_actual
void setCanceled(const Result &result=Result(), const std::string &text=std::string(""))
void onGoal(GoalHandle gh)
void setAccepted(const std::string &text=std::string(""))
bool validate(GoalHandle &gh, Result &res)
std::array< double, 6 > qd_actual_
void setSucceeded(const Result &result=Result(), const std::string &text=std::string(""))
virtual void onRobotStateChange(RobotState state)
std::array< double, 6 > q_actual
bool updateState(RTShared &data)
#define LOG_INFO(format,...)
ActionServer(ActionTrajectoryFollowerInterface &follower, std::vector< std::string > &joint_names, double max_velocity)
std::vector< unsigned int > mapping(const T &t1, const T &t2)
void setAborted(const Result &result=Result(), const std::string &text=std::string(""))
std::vector< std::string > joint_names_
bool try_execute(GoalHandle &gh, Result &res)
bool validateTrajectory(GoalHandle &gh, Result &res)
std::set< std::string > joint_set_
std::chrono::microseconds convert(const ros::Duration &dur)
std::vector< size_t > reorderMap(std::vector< std::string > goal_joints)
#define LOG_ERROR(format,...)
std::atomic< bool > interrupt_traj_
void onCancel(GoalHandle gh)
ActionTrajectoryFollowerInterface & follower_
#define LOG_WARN(format,...)
std::array< double, 6 > q_actual_