From 8d845c6f38a60807dac1a554ed569f471251a9e4 Mon Sep 17 00:00:00 2001 From: Simon Rasmussen Date: Fri, 19 May 2017 01:20:30 +0200 Subject: [PATCH] Action server improvements --- include/ur_modern_driver/ros/action_server.h | 31 +++--- src/ros/action_server.cpp | 100 +++++++++++++++++-- 2 files changed, 110 insertions(+), 21 deletions(-) diff --git a/include/ur_modern_driver/ros/action_server.h b/include/ur_modern_driver/ros/action_server.h index 398e4d5..bf876dc 100644 --- a/include/ur_modern_driver/ros/action_server.h +++ b/include/ur_modern_driver/ros/action_server.h @@ -1,40 +1,40 @@ #pragma once +#include +#include +#include +#include #include #include #include #include #include #include -#include -#include -#include -#include #include "ur_modern_driver/log.h" +#include "ur_modern_driver/ros/service_stopper.h" +#include "ur_modern_driver/ros/trajectory_follower.h" #include "ur_modern_driver/ur/consumer.h" #include "ur_modern_driver/ur/master_board.h" #include "ur_modern_driver/ur/state.h" -#include "ur_modern_driver/ros/service_stopper.h" -#include "ur_modern_driver/ros/trajectory_follower.h" - -class ActionServer : public Service //,public URRTPacketConsumer +class ActionServer : public Service, public IConsumer { private: typedef control_msgs::FollowJointTrajectoryAction Action; typedef control_msgs::FollowJointTrajectoryResult Result; typedef actionlib::ServerGoalHandle GoalHandle; typedef actionlib::ActionServer Server; - + ros::NodeHandle nh_; Server as_; std::vector joint_names_; std::set joint_set_; double max_velocity_; - RobotState state_; - + RobotState state_; + std::array q_actual_, qd_actual_; + GoalHandle curr_gh_; std::atomic interrupt_traj_; std::atomic has_goal_, running_; @@ -48,7 +48,7 @@ private: void onCancel(GoalHandle gh); bool validate(GoalHandle& gh, Result& res); - bool validateState(GoalHandle& gh, Result& res); + bool validateState(GoalHandle& gh, Result& res); bool validateJoints(GoalHandle& gh, Result& res); bool validateTrajectory(GoalHandle& gh, Result& res); @@ -58,11 +58,16 @@ private: std::vector reorderMap(std::vector goal_joints); void trajectoryThread(); - + bool updateState(RTShared& data); public: ActionServer(TrajectoryFollower& follower, std::vector& joint_names, double max_velocity); void start(); virtual void onRobotStateChange(RobotState state); + + virtual bool consume(RTState_V1_6__7& state); + virtual bool consume(RTState_V1_8& state); + virtual bool consume(RTState_V3_0__1& state); + virtual bool consume(RTState_V3_2__3& state); }; \ No newline at end of file diff --git a/src/ros/action_server.cpp b/src/ros/action_server.cpp index 629a50f..1ec7217 100644 --- a/src/ros/action_server.cpp +++ b/src/ros/action_server.cpp @@ -12,6 +12,9 @@ ActionServer::ActionServer(TrajectoryFollower& follower, std::vector lock(tj_mutex_); + + Result res; + res.error_code = -100; + res.error_string = "Received another trajectory"; + curr_gh_.setAborted(res, res.error_string); } + +bool ActionServer::updateState(RTShared& data) +{ + q_actual_ = data.q_actual; + qd_actual_ = data.qd_actual; + return true; +} + + +bool ActionServer::consume(RTState_V1_6__7& state) +{ + return updateState(state); +} +bool ActionServer::consume(RTState_V1_8& state) +{ + return updateState(state); +} +bool ActionServer::consume(RTState_V3_0__1& state) +{ + return updateState(state); +} +bool ActionServer::consume(RTState_V3_2__3& state) +{ + return updateState(state); +} + + void ActionServer::onGoal(GoalHandle gh) { Result res; res.error_code = -100; + LOG_INFO("Received new goal"); + if(!validate(gh, res) || !try_execute(gh, res)) + { + LOG_WARN("Goal error: %s", res.error_string.c_str()); gh.setRejected(res, res.error_string); + } } void ActionServer::onCancel(GoalHandle gh) @@ -53,7 +115,7 @@ void ActionServer::onCancel(GoalHandle gh) bool ActionServer::validate(GoalHandle& gh, Result& res) { - return !validateState(gh, res) || !validateJoints(gh, res) || !validateTrajectory(gh, res); + return validateState(gh, res) && validateJoints(gh, res) && validateTrajectory(gh, res); } bool ActionServer::validateState(GoalHandle& gh, Result& res) @@ -100,6 +162,10 @@ 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) + return false; + for(auto const& point : goal->trajectory.points) { if(point.velocities.size() != joint_names_.size()) @@ -194,28 +260,41 @@ std::vector ActionServer::reorderMap(std::vector goal_joint void ActionServer::trajectoryThread() { + LOG_INFO("Trajectory thread started"); follower_.start(); //todo check error - //as_.start(); while(running_) { std::unique_lock lk(tj_mutex_); if(!tj_cv_.wait_for(lk, std::chrono::milliseconds(100), [&]{return running_ && has_goal_;})) continue; - LOG_DEBUG("Trajectory received and accepted"); + LOG_INFO("Trajectory received and accepted"); curr_gh_.setAccepted(); auto goal = curr_gh_.getGoal(); - auto mapping = reorderMap(goal->trajectory.joint_names); std::vector trajectory(goal->trajectory.points.size()); + + //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)) + { + 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) { std::array pos, vel; for(size_t i = 0; i < 6; i++) { - //joint names of the goal might have a different ordering compared - //to what URScript expects so need to map between the two size_t idx = mapping[i]; pos[idx] = point.positions[i]; vel[idx] = point.velocities[i]; @@ -223,20 +302,25 @@ void ActionServer::trajectoryThread() trajectory.push_back(TrajectoryPoint(pos, vel, convert(point.time_from_start))); } + double t = std::chrono::duration_cast>(trajectory[trajectory.size()-1].time_from_start).count(); + LOG_INFO("Trajectory with %d points and duration of %f constructed, executing now", trajectory.size(), t); + Result res; if(follower_.execute(trajectory, interrupt_traj_)) { //interrupted goals must be handled by interrupt trigger if(!interrupt_traj_) { - LOG_DEBUG("Trajectory executed successfully"); + LOG_INFO("Trajectory executed successfully"); res.error_code = Result::SUCCESSFUL; curr_gh_.setSucceeded(res); } + else + LOG_INFO("Trajectory interrupted"); } else { - LOG_DEBUG("Trajectory failed"); + LOG_INFO("Trajectory failed"); res.error_code = -100; res.error_string = "Connection to robot was lost"; curr_gh_.setAborted(res, res.error_string);