From b63869c56c0ea213b0662a5e0d2743aa89dd39a3 Mon Sep 17 00:00:00 2001 From: Ethan Gordon Date: Mon, 29 Jun 2020 16:55:16 -0700 Subject: [PATCH 1/6] Added cartesian velocity control function --- include/libada/Ada.hpp | 45 ++++++++++++ src/Ada.cpp | 157 +++++++++++++++++++++++++++++++++++++++++ 2 files changed, 202 insertions(+) diff --git a/include/libada/Ada.hpp b/include/libada/Ada.hpp index 3b6d4ab..32ed0a7 100644 --- a/include/libada/Ada.hpp +++ b/include/libada/Ada.hpp @@ -27,6 +27,9 @@ #include #include +#include +#include + #include "libada/AdaHand.hpp" namespace ada { @@ -254,6 +257,28 @@ class Ada final : public aikido::robot::Robot /// \return true if all controllers have been successfully switched bool stopTrajectoryExecutor(); + /// Toggle controllers and enable/disable cartesian velocity control. + /// Required before calling \c moveArmCommandVelocity + /// \return true if all controllers have been successfully switched + bool setVelocityControl(bool enabled); + + /// Moves the end effector at a cartesian velocity for a specified + /// amount of time. + /// \param[in] linear, 3-D velocity in world space, m/s + /// \param[in] angular, 3-D velocity about world axes, rad/s + /// \param[in] forTime, maximum time to execute velocity + /// \param[in] block, block until completed, cancelled, or aborted + /// \return True if the trajectory was completed successfully. + std::future moveArmCommandVelocity( + const Eigen::Vector3d& linear, + const Eigen::Vector3d& angular = Eigen::Vector3d(0.0, 0.0, 0.0), + ros::Duration forTime = ros::Duration(1.0), + bool block = false); + + /// Stops any movement created by \c moveArmCommandVelocity + /// \return True on successful cancellation + bool cancelCommandVelocity(); + /// \copydoc ConcreteRobot::setCRRTPlannerParameters void setCRRTPlannerParameters( const aikido::robot::util::CRRTPlannerParameters& crrtParameters); @@ -355,6 +380,14 @@ class Ada final : public aikido::robot::Robot const std::vector& startControllers, const std::vector& stopControllers); +// Short-cuts for action client types +protected: + using Action = pr_control_msgs::SetCartesianVelocityAction; + using ActionClient = actionlib::ActionClient; + using GoalHandle = ActionClient::GoalHandle; + + using Result = pr_control_msgs::SetCartesianVelocityResult; + private: // Named Configurations are read from a YAML file using ConfigurationMap = std::unordered_map; @@ -461,6 +494,18 @@ class Ada final : public aikido::robot::Robot // For trajectory executions. std::unique_ptr mThread; + + ////// Cartesian Velocity Variables + // Name of cartesian velocity action server + const std::string mCartesianVelocityName = "move_until_touch_cartvel_controller"; + // Action Client + std::shared_ptr mActionClient; + // Transition callback for velocity goals + void transitionCallback(GoalHandle handle); + // Current action goal + GoalHandle mGoalHandle; + // Promise to keep track of current velocity goal + std::shared_ptr> mPromise; }; } // namespace ada diff --git a/src/Ada.cpp b/src/Ada.cpp index 95f3e63..3859d98 100644 --- a/src/Ada.cpp +++ b/src/Ada.cpp @@ -254,6 +254,11 @@ Ada::Ada( selfCollisionFilter); mRobot->setCRRTPlannerParameters(crrtParams); + // Start Cartesian Velocity Action Client + if (!mSimulation) { + mActionClient = std::make_shared(mCartesianVelocityName + "/cart_velocity"); + } + // TODO: When the named configurations are set in resources. // Load the named configurations // auto namedConfigurations = parseYAMLToNamedConfigurations( @@ -526,6 +531,158 @@ bool Ada::stopTrajectoryExecutor() mHandTrajectoryExecutorName}); } +//============================================================================= +bool Ada::setVelocityControl(bool enabled) { + if(mSimulation) return false; + bool ret; + + controller_manager_msgs::SwitchController srv; + srv.request.strictness + = controller_manager_msgs::SwitchControllerRequest::STRICT; + + if(enabled) { + // Stop joint trajectory control + ret = stopTrajectoryExecutor(); + if (!ret) return false; + + // Switch controllers + srv.request.start_controllers = std::vector{mCartesianVelocityName}; + srv.request.stop_controllers = std::vector(); + if (!mControllerServiceClient->call(srv) || !srv.response.ok) { + return false; + } + + // Ensure action client is running + return mActionClient->waitForActionServerToStart(ros::Duration(10.0)); + } else { + // Kill action goals + ret = cancelCommandVelocity(); + if (!ret) return false; + + // Switch controllers + srv.request.start_controllers = std::vector(); + srv.request.stop_controllers = std::vector{mCartesianVelocityName}; + if (!mControllerServiceClient->call(srv) || !srv.response.ok) { + return false; + } + + // Start joint trajectory control + return startTrajectoryExecutor(); + } +} + +//============================================================================= +std::future Ada::moveArmCommandVelocity( + const Eigen::Vector3d& linear, + const Eigen::Vector3d& angular, + ros::Duration forTime, + bool block) { + mPromise.reset(new std::promise()); + std::future ret = mPromise->get_future(); + if (mSimulation) { + mPromise->set_value(false); + return ret; + } + + // Construct goal + pr_control_msgs::SetCartesianVelocityGoal goal; + goal.exec_time = forTime; + goal.command.linear.x = linear(0); + goal.command.linear.y = linear(1); + goal.command.linear.z = linear(2); + goal.command.angular.x = angular(0); + goal.command.angular.y = angular(1); + goal.command.angular.z = angular(2); + + // Send goal + using std::placeholders::_1; + mGoalHandle = mActionClient->sendGoal(goal, + std::bind(&Ada::transitionCallback, this, _1)); + + // Block until done or time-out + if(block) { + std::future_status status = ret.wait_for(std::chrono::nanoseconds(forTime.toNSec())); + if (status != std::future_status::ready) { + // Time-out, write false + mPromise->set_value(false); + } + } + + return ret; +} + +void Ada::transitionCallback(GoalHandle handle) { + // Only handle current goal + if (handle != mGoalHandle) return; + + using actionlib::TerminalState; + + if (handle.getCommState() == actionlib::CommState::DONE) + { + std::stringstream message; + bool isSuccessful = true; + + // Check the status of the actionlib call. Note that the actionlib call can + // succeed, even if execution failed. + const auto terminalState = handle.getTerminalState(); + if (terminalState != TerminalState::SUCCEEDED) + { + message << "Velocity Command " << terminalState.toString(); + + const auto terminalMessage = terminalState.getText(); + if (!terminalMessage.empty()) + message << " (" << terminalMessage << ")"; + + isSuccessful = false; + } + else + { + message << "Execution failed."; + } + + // Check the status of execution. This is only possible if the actionlib + // call succeeded. + const auto result = handle.getResult(); + if (result && result->error_code != Result::SUCCESSFUL) + { + message << ": "; + switch(result->error_code) { + case Result::INVALID_CMD: + message << "INVALID_CMD"; + break; + case Result::FORCE_THRESH: + message << "FORCE_THRESH"; + break; + case Result::CANCELLED: + message << "CANCELLED"; + break; + default: + message << "UNKNOWN"; + } + + if (!result->error_string.empty()) + message << " (" << result->error_string << ")"; + + isSuccessful = false; + } + + if(!isSuccessful) { + ROS_WARN_STREAM(message.str()); + } + + mPromise->set_value(isSuccessful); + } + +} + +//============================================================================= +bool Ada::cancelCommandVelocity() { + if(mSimulation) return false; + if(!mActionClient->isServerConnected()) return false; + mActionClient->cancelAllGoals(); + return true; +} + //============================================================================= void Ada::setCRRTPlannerParameters(const CRRTPlannerParameters& crrtParameters) { From 93eea366b001823443bf5bf73b2a06aeebc1ee8c Mon Sep 17 00:00:00 2001 From: Ethan Gordon Date: Tue, 30 Jun 2020 16:23:57 -0700 Subject: [PATCH 2/6] Add spinner for action client callbacks --- include/libada/Ada.hpp | 2 ++ src/Ada.cpp | 13 +++++++++++-- 2 files changed, 13 insertions(+), 2 deletions(-) diff --git a/include/libada/Ada.hpp b/include/libada/Ada.hpp index 32ed0a7..a75314e 100644 --- a/include/libada/Ada.hpp +++ b/include/libada/Ada.hpp @@ -506,6 +506,8 @@ class Ada final : public aikido::robot::Robot GoalHandle mGoalHandle; // Promise to keep track of current velocity goal std::shared_ptr> mPromise; + // Spinner for callbacks + ros::AsyncSpinner mSpinner; }; } // namespace ada diff --git a/src/Ada.cpp b/src/Ada.cpp index 3859d98..cda81d0 100644 --- a/src/Ada.cpp +++ b/src/Ada.cpp @@ -119,6 +119,7 @@ Ada::Ada( , mSmootherFeasibilityApproxTolerance(1e-3) , mWorld(std::move(env)) , mEndEffectorName(endEffectorName) + , mSpinner(1) { simulation = true; // temporarily set simulation to true @@ -552,9 +553,15 @@ bool Ada::setVelocityControl(bool enabled) { return false; } + // Begin receiving callbacks + mSpinner.start(); + // Ensure action client is running + ROS_INFO_STREAM("Starting cartesian velocity action client..."); return mActionClient->waitForActionServerToStart(ros::Duration(10.0)); } else { + // Stop callbacks + mSpinner.stop(); // Kill action goals ret = cancelCommandVelocity(); if (!ret) return false; @@ -806,8 +813,10 @@ bool Ada::switchControllers( if (mControllerServiceClient->call(srv) && srv.response.ok) { - mArmTrajectoryExecutorName = startControllers[0]; - mTrajectoryExecutor = createTrajectoryExecutor(); + if(startControllers.size() > 0) { + mArmTrajectoryExecutorName = startControllers[0]; + mTrajectoryExecutor = createTrajectoryExecutor(); + } ROS_INFO_STREAM("Controllers switched"); return true; } From 41076371e91e972b4c8846ddd80228dbf9c8170e Mon Sep 17 00:00:00 2001 From: Ethan Gordon Date: Tue, 30 Jun 2020 17:27:13 -0700 Subject: [PATCH 3/6] Add optional FT control for cartesian velocity control --- include/libada/Ada.hpp | 4 +++- src/Ada.cpp | 14 ++++++-------- 2 files changed, 9 insertions(+), 9 deletions(-) diff --git a/include/libada/Ada.hpp b/include/libada/Ada.hpp index a75314e..651d3bd 100644 --- a/include/libada/Ada.hpp +++ b/include/libada/Ada.hpp @@ -259,8 +259,9 @@ class Ada final : public aikido::robot::Robot /// Toggle controllers and enable/disable cartesian velocity control. /// Required before calling \c moveArmCommandVelocity + /// \param[in] useFT whether to use the FT-enabled controller or not. /// \return true if all controllers have been successfully switched - bool setVelocityControl(bool enabled); + bool setVelocityControl(bool enabled, bool useFT = false); /// Moves the end effector at a cartesian velocity for a specified /// amount of time. @@ -498,6 +499,7 @@ class Ada final : public aikido::robot::Robot ////// Cartesian Velocity Variables // Name of cartesian velocity action server const std::string mCartesianVelocityName = "move_until_touch_cartvel_controller"; + const std::string mCartesianVelocityNoFTName = "cartvel_controller"; // Action Client std::shared_ptr mActionClient; // Transition callback for velocity goals diff --git a/src/Ada.cpp b/src/Ada.cpp index cda81d0..f5a2108 100644 --- a/src/Ada.cpp +++ b/src/Ada.cpp @@ -255,11 +255,6 @@ Ada::Ada( selfCollisionFilter); mRobot->setCRRTPlannerParameters(crrtParams); - // Start Cartesian Velocity Action Client - if (!mSimulation) { - mActionClient = std::make_shared(mCartesianVelocityName + "/cart_velocity"); - } - // TODO: When the named configurations are set in resources. // Load the named configurations // auto namedConfigurations = parseYAMLToNamedConfigurations( @@ -533,7 +528,7 @@ bool Ada::stopTrajectoryExecutor() } //============================================================================= -bool Ada::setVelocityControl(bool enabled) { +bool Ada::setVelocityControl(bool enabled, bool useFT) { if(mSimulation) return false; bool ret; @@ -541,13 +536,16 @@ bool Ada::setVelocityControl(bool enabled) { srv.request.strictness = controller_manager_msgs::SwitchControllerRequest::STRICT; + const std::string controlName = useFT ? mCartesianVelocityName : mCartesianVelocityNoFTName; + if(enabled) { + mActionClient = std::make_shared(controlName + "/cart_velocity"); // Stop joint trajectory control ret = stopTrajectoryExecutor(); if (!ret) return false; // Switch controllers - srv.request.start_controllers = std::vector{mCartesianVelocityName}; + srv.request.start_controllers = std::vector{controlName}; srv.request.stop_controllers = std::vector(); if (!mControllerServiceClient->call(srv) || !srv.response.ok) { return false; @@ -568,7 +566,7 @@ bool Ada::setVelocityControl(bool enabled) { // Switch controllers srv.request.start_controllers = std::vector(); - srv.request.stop_controllers = std::vector{mCartesianVelocityName}; + srv.request.stop_controllers = std::vector{controlName}; if (!mControllerServiceClient->call(srv) || !srv.response.ok) { return false; } From 7da081582a11e58102c6348c03694b81da0651ec Mon Sep 17 00:00:00 2001 From: Ethan Gordon Date: Wed, 1 Jul 2020 18:33:12 -0700 Subject: [PATCH 4/6] Add richer feedback on velocity commands --- include/libada/Ada.hpp | 17 ++++++++++++++-- src/Ada.cpp | 46 +++++++++++++++++++++++++----------------- 2 files changed, 43 insertions(+), 20 deletions(-) diff --git a/include/libada/Ada.hpp b/include/libada/Ada.hpp index 651d3bd..7d90987 100644 --- a/include/libada/Ada.hpp +++ b/include/libada/Ada.hpp @@ -47,6 +47,17 @@ enum TrajectoryPostprocessType KUNZ }; +// Enum for Cartesian Velocity Return Result +enum CartVelocityResult +{ + kCVR_SUCCESS, + kCVR_INVALID, + kCVR_FORCE, + kCVR_CANCELLED, + kCVR_TIMEOUT, + kCVR_UNKNOWN +}; + class Ada final : public aikido::robot::Robot { public: @@ -270,7 +281,7 @@ class Ada final : public aikido::robot::Robot /// \param[in] forTime, maximum time to execute velocity /// \param[in] block, block until completed, cancelled, or aborted /// \return True if the trajectory was completed successfully. - std::future moveArmCommandVelocity( + std::future moveArmCommandVelocity( const Eigen::Vector3d& linear, const Eigen::Vector3d& angular = Eigen::Vector3d(0.0, 0.0, 0.0), ros::Duration forTime = ros::Duration(1.0), @@ -507,9 +518,11 @@ class Ada final : public aikido::robot::Robot // Current action goal GoalHandle mGoalHandle; // Promise to keep track of current velocity goal - std::shared_ptr> mPromise; + std::shared_ptr> mPromise; // Spinner for callbacks ros::AsyncSpinner mSpinner; + // Track whether using velocity control + bool mVelocityControl; }; } // namespace ada diff --git a/src/Ada.cpp b/src/Ada.cpp index f5a2108..74cd736 100644 --- a/src/Ada.cpp +++ b/src/Ada.cpp @@ -120,6 +120,7 @@ Ada::Ada( , mWorld(std::move(env)) , mEndEffectorName(endEffectorName) , mSpinner(1) + , mVelocityControl(false) { simulation = true; // temporarily set simulation to true @@ -530,6 +531,9 @@ bool Ada::stopTrajectoryExecutor() //============================================================================= bool Ada::setVelocityControl(bool enabled, bool useFT) { if(mSimulation) return false; + // Do not re-enable or re-disable + if(mVelocityControl == enabled) return true; + bool ret; controller_manager_msgs::SwitchController srv; @@ -577,15 +581,15 @@ bool Ada::setVelocityControl(bool enabled, bool useFT) { } //============================================================================= -std::future Ada::moveArmCommandVelocity( +std::future Ada::moveArmCommandVelocity( const Eigen::Vector3d& linear, const Eigen::Vector3d& angular, ros::Duration forTime, bool block) { - mPromise.reset(new std::promise()); - std::future ret = mPromise->get_future(); - if (mSimulation) { - mPromise->set_value(false); + mPromise.reset(new std::promise()); + std::future ret = mPromise->get_future(); + if (mSimulation || !mVelocityControl) { + mPromise->set_value(kCVR_INVALID); return ret; } @@ -609,7 +613,7 @@ std::future Ada::moveArmCommandVelocity( std::future_status status = ret.wait_for(std::chrono::nanoseconds(forTime.toNSec())); if (status != std::future_status::ready) { // Time-out, write false - mPromise->set_value(false); + mPromise->set_value(kCVR_TIMEOUT); } } @@ -625,28 +629,21 @@ void Ada::transitionCallback(GoalHandle handle) { if (handle.getCommState() == actionlib::CommState::DONE) { std::stringstream message; - bool isSuccessful = true; + CartVelocityResult isSuccessful = kCVR_SUCCESS; // Check the status of the actionlib call. Note that the actionlib call can // succeed, even if execution failed. const auto terminalState = handle.getTerminalState(); if (terminalState != TerminalState::SUCCEEDED) { - message << "Velocity Command " << terminalState.toString(); - - const auto terminalMessage = terminalState.getText(); - if (!terminalMessage.empty()) - message << " (" << terminalMessage << ")"; - - isSuccessful = false; + } else { message << "Execution failed."; } - // Check the status of execution. This is only possible if the actionlib - // call succeeded. + // Check the status of execution. const auto result = handle.getResult(); if (result && result->error_code != Result::SUCCESSFUL) { @@ -654,24 +651,36 @@ void Ada::transitionCallback(GoalHandle handle) { switch(result->error_code) { case Result::INVALID_CMD: message << "INVALID_CMD"; + isSuccessful = kCVR_INVALID; break; case Result::FORCE_THRESH: message << "FORCE_THRESH"; + isSuccessful = kCVR_FORCE; break; case Result::CANCELLED: message << "CANCELLED"; + isSuccessful = kCVR_CANCELLED; break; default: message << "UNKNOWN"; + isSuccessful = kCVR_UNKNOWN; } if (!result->error_string.empty()) message << " (" << result->error_string << ")"; + } + // we can fail without a result for another reason + else if (!result) { + message << "Velocity Command " << terminalState.toString(); + + const auto terminalMessage = terminalState.getText(); + if (!terminalMessage.empty()) + message << " (" << terminalMessage << ")"; - isSuccessful = false; + isSuccessful = kCVR_UNKNOWN; } - if(!isSuccessful) { + if(isSuccessful != kCVR_SUCCESS) { ROS_WARN_STREAM(message.str()); } @@ -684,6 +693,7 @@ void Ada::transitionCallback(GoalHandle handle) { bool Ada::cancelCommandVelocity() { if(mSimulation) return false; if(!mActionClient->isServerConnected()) return false; + if(!mVelocityControl) return false; mActionClient->cancelAllGoals(); return true; } From a9d5e9f0d5db98c4165025737a96827e465556aa Mon Sep 17 00:00:00 2001 From: Ethan Gordon Date: Thu, 2 Jul 2020 20:08:52 -0700 Subject: [PATCH 5/6] Some better thread stability --- src/Ada.cpp | 27 +++++++++++++++++++-------- 1 file changed, 19 insertions(+), 8 deletions(-) diff --git a/src/Ada.cpp b/src/Ada.cpp index 74cd736..7dab144 100644 --- a/src/Ada.cpp +++ b/src/Ada.cpp @@ -264,6 +264,9 @@ Ada::Ada( mThread = std::make_unique( std::bind(&Ada::update, this), threadExecutionCycle); + + // Callbacks for Cartesian Velocity + mSpinner.start(); } //============================================================================== @@ -555,15 +558,14 @@ bool Ada::setVelocityControl(bool enabled, bool useFT) { return false; } - // Begin receiving callbacks - mSpinner.start(); - // Ensure action client is running ROS_INFO_STREAM("Starting cartesian velocity action client..."); - return mActionClient->waitForActionServerToStart(ros::Duration(10.0)); + if(!mActionClient->waitForActionServerToStart(ros::Duration(10.0))) { + return false; + } + mVelocityControl = true; + return true; } else { - // Stop callbacks - mSpinner.stop(); // Kill action goals ret = cancelCommandVelocity(); if (!ret) return false; @@ -576,7 +578,11 @@ bool Ada::setVelocityControl(bool enabled, bool useFT) { } // Start joint trajectory control - return startTrajectoryExecutor(); + if(!startTrajectoryExecutor()) { + return false; + } + mVelocityControl = false; + return true; } } @@ -623,6 +629,7 @@ std::future Ada::moveArmCommandVelocity( void Ada::transitionCallback(GoalHandle handle) { // Only handle current goal if (handle != mGoalHandle) return; + if (!mVelocityControl) return; using actionlib::TerminalState; @@ -684,7 +691,11 @@ void Ada::transitionCallback(GoalHandle handle) { ROS_WARN_STREAM(message.str()); } - mPromise->set_value(isSuccessful); + try { + mPromise->set_value(isSuccessful); + } catch(...) { + ROS_WARN_STREAM("Promise already satisfied."); + } } } From d92fa70bea6b102d86ffeb9be7a5d44bb029a423 Mon Sep 17 00:00:00 2001 From: Ethan Gordon Date: Fri, 3 Jul 2020 13:19:55 -0700 Subject: [PATCH 6/6] Switched to SimpleActionClient --- include/libada/Ada.hpp | 13 +++++-------- src/Ada.cpp | 41 ++++++++++++++--------------------------- 2 files changed, 19 insertions(+), 35 deletions(-) diff --git a/include/libada/Ada.hpp b/include/libada/Ada.hpp index 7d90987..9a6b196 100644 --- a/include/libada/Ada.hpp +++ b/include/libada/Ada.hpp @@ -28,7 +28,7 @@ #include #include -#include +#include #include "libada/AdaHand.hpp" @@ -395,10 +395,11 @@ class Ada final : public aikido::robot::Robot // Short-cuts for action client types protected: using Action = pr_control_msgs::SetCartesianVelocityAction; - using ActionClient = actionlib::ActionClient; - using GoalHandle = ActionClient::GoalHandle; + using ActionClient = actionlib::SimpleActionClient; + using GoalState = actionlib::SimpleClientGoalState; using Result = pr_control_msgs::SetCartesianVelocityResult; + using ResultPtr = pr_control_msgs::SetCartesianVelocityResultConstPtr; private: // Named Configurations are read from a YAML file @@ -514,13 +515,9 @@ class Ada final : public aikido::robot::Robot // Action Client std::shared_ptr mActionClient; // Transition callback for velocity goals - void transitionCallback(GoalHandle handle); - // Current action goal - GoalHandle mGoalHandle; + void doneCallback(const GoalState& handle, const ResultPtr& result); // Promise to keep track of current velocity goal std::shared_ptr> mPromise; - // Spinner for callbacks - ros::AsyncSpinner mSpinner; // Track whether using velocity control bool mVelocityControl; }; diff --git a/src/Ada.cpp b/src/Ada.cpp index 7dab144..971a774 100644 --- a/src/Ada.cpp +++ b/src/Ada.cpp @@ -119,7 +119,6 @@ Ada::Ada( , mSmootherFeasibilityApproxTolerance(1e-3) , mWorld(std::move(env)) , mEndEffectorName(endEffectorName) - , mSpinner(1) , mVelocityControl(false) { simulation = true; // temporarily set simulation to true @@ -264,9 +263,6 @@ Ada::Ada( mThread = std::make_unique( std::bind(&Ada::update, this), threadExecutionCycle); - - // Callbacks for Cartesian Velocity - mSpinner.start(); } //============================================================================== @@ -560,7 +556,7 @@ bool Ada::setVelocityControl(bool enabled, bool useFT) { // Ensure action client is running ROS_INFO_STREAM("Starting cartesian velocity action client..."); - if(!mActionClient->waitForActionServerToStart(ros::Duration(10.0))) { + if(!mActionClient->waitForServer(ros::Duration(10.0))) { return false; } mVelocityControl = true; @@ -611,8 +607,12 @@ std::future Ada::moveArmCommandVelocity( // Send goal using std::placeholders::_1; - mGoalHandle = mActionClient->sendGoal(goal, - std::bind(&Ada::transitionCallback, this, _1)); + using std::placeholders::_2; + if (mActionClient->getState() != actionlib::SimpleClientGoalState::LOST) { + mActionClient->stopTrackingGoal(); + } + mActionClient->sendGoal(goal, + std::bind(&Ada::doneCallback, this, _1, _2)); // Block until done or time-out if(block) { @@ -626,32 +626,16 @@ std::future Ada::moveArmCommandVelocity( return ret; } -void Ada::transitionCallback(GoalHandle handle) { +void Ada::doneCallback(const GoalState& handle, const ResultPtr& result) { // Only handle current goal - if (handle != mGoalHandle) return; if (!mVelocityControl) return; - using actionlib::TerminalState; - - if (handle.getCommState() == actionlib::CommState::DONE) + if (handle.isDone()) { std::stringstream message; CartVelocityResult isSuccessful = kCVR_SUCCESS; - // Check the status of the actionlib call. Note that the actionlib call can - // succeed, even if execution failed. - const auto terminalState = handle.getTerminalState(); - if (terminalState != TerminalState::SUCCEEDED) - { - - } - else - { - message << "Execution failed."; - } - // Check the status of execution. - const auto result = handle.getResult(); if (result && result->error_code != Result::SUCCESSFUL) { message << ": "; @@ -678,9 +662,9 @@ void Ada::transitionCallback(GoalHandle handle) { } // we can fail without a result for another reason else if (!result) { - message << "Velocity Command " << terminalState.toString(); + message << "Velocity Command " << handle.toString(); - const auto terminalMessage = terminalState.getText(); + const auto terminalMessage = handle.getText(); if (!terminalMessage.empty()) message << " (" << terminalMessage << ")"; @@ -705,6 +689,9 @@ bool Ada::cancelCommandVelocity() { if(mSimulation) return false; if(!mActionClient->isServerConnected()) return false; if(!mVelocityControl) return false; + if (mActionClient->getState() != actionlib::SimpleClientGoalState::LOST) { + mActionClient->stopTrackingGoal(); + } mActionClient->cancelAllGoals(); return true; }