From bafd41721a74bfb14eafb7f8990b2ec053ce4ace Mon Sep 17 00:00:00 2001 From: Jan Krieglstein Date: Wed, 26 Apr 2023 14:38:54 +0200 Subject: [PATCH 01/11] Consider AXIS_BLOCKED as error when happening during grasping --- ipa325_wsg50/src/WSG50ROSNode.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/ipa325_wsg50/src/WSG50ROSNode.cpp b/ipa325_wsg50/src/WSG50ROSNode.cpp index 32bab59..a2823de 100644 --- a/ipa325_wsg50/src/WSG50ROSNode.cpp +++ b/ipa325_wsg50/src/WSG50ROSNode.cpp @@ -309,6 +309,7 @@ class WSG50GraspPartActionServer : public WSG50RosObserver response->status_code==E_RANGE_ERROR || response->status_code==E_CMD_ABORTED || response->status_code==E_CMD_FAILED || + response->status_code==E_AXIS_BLOCKED || response->status_code==E_TIMEOUT) { gpserver_.setAborted(res_); _controller->Detach(this, 0x43); From 9f15aede67e66e797bb42fd56b4bd6fb171e1206 Mon Sep 17 00:00:00 2001 From: Jan Krieglstein Date: Wed, 26 Apr 2023 14:39:41 +0200 Subject: [PATCH 02/11] Wait longer for establishing connection - 10ms is not enough on slow pcs --- ipa325_wsg50/src/WSG50Controller.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ipa325_wsg50/src/WSG50Controller.cpp b/ipa325_wsg50/src/WSG50Controller.cpp index 2d9facc..3b049dd 100644 --- a/ipa325_wsg50/src/WSG50Controller.cpp +++ b/ipa325_wsg50/src/WSG50Controller.cpp @@ -280,7 +280,7 @@ void WSG50Controller::setupConnection() // need to wait certain time, otherwise connection won't be established. // - std::this_thread::sleep_for(std::chrono::milliseconds(10)); + std::this_thread::sleep_for(std::chrono::milliseconds(1000)); // checking connection state // From 5c0c8da6dab1581a19fb29d795e6bb2f773f9142 Mon Sep 17 00:00:00 2001 From: Jan Krieglstein Date: Wed, 26 Apr 2023 14:26:36 +0200 Subject: [PATCH 03/11] Decrease verbosity Disable debug mode by default --- ipa325_wsg50/src/WSG50Communicator.cpp | 8 ++++---- ipa325_wsg50/src/WSG50Controller.cpp | 2 +- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/ipa325_wsg50/src/WSG50Communicator.cpp b/ipa325_wsg50/src/WSG50Communicator.cpp index a91e5a2..d789602 100644 --- a/ipa325_wsg50/src/WSG50Communicator.cpp +++ b/ipa325_wsg50/src/WSG50Communicator.cpp @@ -8,7 +8,7 @@ using namespace boost; using namespace std; -#define DEBUG 3 +#define DEBUG 0 #ifndef SER_MSG_NUM_HEADER_BYTES #define SER_MSG_NUM_HEADER_BYTES 3 //!< number of header bytes @@ -370,7 +370,7 @@ void WSG50Communicator::read_handler(const boost::system::error_code &ec, // ROS_ERROR("response message length = %d", responseMsg.length); // check if response-message-length == -1, then there was an error creating the message if(responseMsg.length == -1) { - ROS_ERROR("could not create TRESPONSE. continue"); + ROS_DEBUG("could not create TRESPONSE. continue"); // clear buffer // @@ -816,8 +816,8 @@ TRESPONSE WSG50Communicator::createTRESPONSE(unsigned char * data, size_t TCPPac // if((int) respMsg.length != ((int) (TCPPacketLength - 10))) { - ROS_ERROR("1 Length of response TCP-message does not match announced data-length!"); - ROS_ERROR("2 actual length = %d, announced data-length = %d", ((int) TCPPacketLength -10), (int) respMsg.length); + ROS_DEBUG("1 Length of response TCP-message does not match announced data-length!"); + ROS_DEBUG("2 actual length = %d, announced data-length = %d", ((int) TCPPacketLength -10), (int) respMsg.length); if(DEBUG) printHexArray(data, TCPPacketLength); respMsg.length = -1; return respMsg; diff --git a/ipa325_wsg50/src/WSG50Controller.cpp b/ipa325_wsg50/src/WSG50Controller.cpp index 3b049dd..320c399 100644 --- a/ipa325_wsg50/src/WSG50Controller.cpp +++ b/ipa325_wsg50/src/WSG50Controller.cpp @@ -10,7 +10,7 @@ * ### Define Default Values ############# * ########################################### */ -#define DEBUG true +#define DEBUG false #define TIMEOUT 1 // sec From 5d67f81d7a201037439c853e482b895f2c0674f2 Mon Sep 17 00:00:00 2001 From: Jan Krieglstein Date: Wed, 26 Apr 2023 14:42:16 +0200 Subject: [PATCH 04/11] Use std empty services instead of empty custom services --- ipa325_wsg50/CMakeLists.txt | 5 +---- ipa325_wsg50/package.xml | 2 ++ ipa325_wsg50/src/WSG50ROSNode.cpp | 21 +++++++++------------ ipa325_wsg50/src/lwr_testclient.cpp | 23 ++++++++++------------- ipa325_wsg50/srv/ackFastStop.srv | 3 --- ipa325_wsg50/srv/clearSoftLimits.srv | 3 --- ipa325_wsg50/srv/fastStop.srv | 3 --- ipa325_wsg50/srv/stop.srv | 3 --- 8 files changed, 22 insertions(+), 41 deletions(-) delete mode 100644 ipa325_wsg50/srv/ackFastStop.srv delete mode 100644 ipa325_wsg50/srv/clearSoftLimits.srv delete mode 100644 ipa325_wsg50/srv/fastStop.srv delete mode 100644 ipa325_wsg50/srv/stop.srv diff --git a/ipa325_wsg50/CMakeLists.txt b/ipa325_wsg50/CMakeLists.txt index 364261f..579dc8e 100644 --- a/ipa325_wsg50/CMakeLists.txt +++ b/ipa325_wsg50/CMakeLists.txt @@ -16,6 +16,7 @@ find_package(catkin sensor_msgs trajectory_msgs message_generation + std_srvs ## additional packages ) @@ -60,14 +61,10 @@ add_message_files( add_service_files( FILES # WSG50Disconnect.srv - clearSoftLimits.srv setAcceleration.srv setForceLimit.srv setSoftLimits.srv tareForceSensor.srv - stop.srv - fastStop.srv - ackFastStop.srv ) ## Generate added messages and services with any dependencies listed here diff --git a/ipa325_wsg50/package.xml b/ipa325_wsg50/package.xml index 2a6dcd8..f8cb041 100644 --- a/ipa325_wsg50/package.xml +++ b/ipa325_wsg50/package.xml @@ -41,6 +41,7 @@ roscpp std_msgs + std_srvs actionlib actionlib_msgs sensor_msgs @@ -49,6 +50,7 @@ roscpp std_msgs + std_srvs actionlib_msgs sensor_msgs trajectory_msgs diff --git a/ipa325_wsg50/src/WSG50ROSNode.cpp b/ipa325_wsg50/src/WSG50ROSNode.cpp index a2823de..9a54bbb 100644 --- a/ipa325_wsg50/src/WSG50ROSNode.cpp +++ b/ipa325_wsg50/src/WSG50ROSNode.cpp @@ -29,11 +29,8 @@ #include #include #include -#include #include -#include -#include -#include +#include // Programm Control // @@ -544,8 +541,8 @@ bool setSoftLimitsService(ipa325_wsg50::setSoftLimits::Request &req, // this will set the minus and plus soft-limits of the schunk gripper // there is no response code, since the values are published in the system states message // -bool clearSoftLimitsService(ipa325_wsg50::clearSoftLimits::Request &req, - ipa325_wsg50::clearSoftLimits::Response &resp) +bool clearSoftLimitsService(std_srvs::Empty::Request &req, + std_srvs::Empty::Response &resp) { ROS_INFO("Clear Soft Limits service called"); @@ -576,8 +573,8 @@ bool setForceLimitService(ipa325_wsg50::setForceLimit::Request &req, // ros service: stop // this will stop gripper movements // -bool stopService(ipa325_wsg50::stop::Request &req, - ipa325_wsg50::stop::Response &resp) +bool stopService(std_srvs::Empty::Request &req, + std_srvs::Empty::Response &resp) { ROS_INFO("STOP service called"); @@ -593,8 +590,8 @@ bool stopService(ipa325_wsg50::stop::Request &req, // in this state the gripper will not accept any other movement orders until // "acknowledgeFastStop" is called! // -bool fastStopService(ipa325_wsg50::stop::Request &req, - ipa325_wsg50::stop::Response &resp) +bool fastStopService(std_srvs::Empty::Request &req, + std_srvs::Empty::Response &resp) { ROS_INFO("FASTSTOP service called"); @@ -608,8 +605,8 @@ bool fastStopService(ipa325_wsg50::stop::Request &req, // ros service: acknowledge fast stop // return gripper into idle state where new commands can be sent. // -bool acknowledgeFastStopService(ipa325_wsg50::stop::Request &req, - ipa325_wsg50::stop::Response &resp) +bool acknowledgeFastStopService(std_srvs::Empty::Request &req, + std_srvs::Empty::Response &resp) { ROS_INFO("Acknowledge fast stop service called"); diff --git a/ipa325_wsg50/src/lwr_testclient.cpp b/ipa325_wsg50/src/lwr_testclient.cpp index 552d3b9..0acdddc 100644 --- a/ipa325_wsg50/src/lwr_testclient.cpp +++ b/ipa325_wsg50/src/lwr_testclient.cpp @@ -23,11 +23,8 @@ #include #include #include -#include #include -#include -#include -#include +#include // contents // @@ -198,8 +195,8 @@ int main(int argc, char **argv) // ROS_INFO("Wait for x seconds"); std::this_thread::sleep_for(std::chrono::milliseconds(2000)); - ros::ServiceClient clearSoftLimitsClient = node.serviceClient("ClearSoftLimits"); - ipa325_wsg50::clearSoftLimits cslMsg; + ros::ServiceClient clearSoftLimitsClient = node.serviceClient("ClearSoftLimits"); + std_srvs::Empty cslMsg; if(clearSoftLimitsClient.call(cslMsg)) ROS_INFO("Clear soft limits service has returned successfull"); else { @@ -268,7 +265,7 @@ int main(int argc, char **argv) return 1; } - // send action goal + // send action goalipa325_wsg50::ackFastStop ipa325_wsg50::WSG50PrePositionFingersGoal goal2; goal2.stopOnBlock = true; goal2.width = 55.0; @@ -286,8 +283,8 @@ int main(int argc, char **argv) if(count == 1000) { // wait 1 sec // send stop message std::this_thread::sleep_for(std::chrono::milliseconds(100)); - ros::ServiceClient stopClient = node.serviceClient("Stop"); - ipa325_wsg50::stop stopMsg; + ros::ServiceClient stopClient = node.serviceClient("Stop"); + std_srvs::Empty stopMsg; ROS_INFO("send stop message"); if(stopClient.call(stopMsg)) ROS_INFO("Stop called successfully"); @@ -324,8 +321,8 @@ int main(int argc, char **argv) if(count == 1000) { // wait 1 sec // send fast stop message std::this_thread::sleep_for(std::chrono::milliseconds(100)); - ros::ServiceClient fastStopClient = node.serviceClient("FastStop"); - ipa325_wsg50::stop fastStopMsg; + ros::ServiceClient fastStopClient = node.serviceClient("FastStop"); + std_srvs::Empty fastStopMsg; ROS_INFO("send stop message"); if(fastStopClient.call(fastStopMsg)) ROS_INFO("FastStop called successfully"); @@ -342,8 +339,8 @@ int main(int argc, char **argv) // std::this_thread::sleep_for(std::chrono::milliseconds(2000)); // wait 2 sec - ros::ServiceClient ackFastStopClient = node.serviceClient("AcknowledgeFastStop"); - ipa325_wsg50::ackFastStop ackMsg; + ros::ServiceClient ackFastStopClient = node.serviceClient("AcknowledgeFastStop"); + std_srvs::Empty ackMsg; ROS_INFO("Acknowledge Fast Stop"); if(ackFastStopClient.call(ackMsg)) ROS_INFO("acknowledgeFastStop successfully called!"); diff --git a/ipa325_wsg50/srv/ackFastStop.srv b/ipa325_wsg50/srv/ackFastStop.srv deleted file mode 100644 index c640784..0000000 --- a/ipa325_wsg50/srv/ackFastStop.srv +++ /dev/null @@ -1,3 +0,0 @@ -# acknowledge fast stop (no params required) ---- -# no response diff --git a/ipa325_wsg50/srv/clearSoftLimits.srv b/ipa325_wsg50/srv/clearSoftLimits.srv deleted file mode 100644 index b27f560..0000000 --- a/ipa325_wsg50/srv/clearSoftLimits.srv +++ /dev/null @@ -1,3 +0,0 @@ -# clear soft limits (no parameters required) ---- -# response no response since included in system-state-message diff --git a/ipa325_wsg50/srv/fastStop.srv b/ipa325_wsg50/srv/fastStop.srv deleted file mode 100644 index 64a9fab..0000000 --- a/ipa325_wsg50/srv/fastStop.srv +++ /dev/null @@ -1,3 +0,0 @@ -# fast stop (no params required) ---- -# no response diff --git a/ipa325_wsg50/srv/stop.srv b/ipa325_wsg50/srv/stop.srv deleted file mode 100644 index 33cb6ee..0000000 --- a/ipa325_wsg50/srv/stop.srv +++ /dev/null @@ -1,3 +0,0 @@ -# stop (no params required) ---- -# no response From ee9ad60d8f73517d32ef820d352f978676032931 Mon Sep 17 00:00:00 2001 From: Jan Krieglstein Date: Wed, 11 Aug 2021 12:53:38 +0200 Subject: [PATCH 05/11] Add gripper joints to urdf --- ipa325_wsg50/urdf/wsg50.urdf.xacro | 22 ++++++++++++++++++++++ 1 file changed, 22 insertions(+) diff --git a/ipa325_wsg50/urdf/wsg50.urdf.xacro b/ipa325_wsg50/urdf/wsg50.urdf.xacro index cc5ccbc..3249c56 100644 --- a/ipa325_wsg50/urdf/wsg50.urdf.xacro +++ b/ipa325_wsg50/urdf/wsg50.urdf.xacro @@ -40,6 +40,28 @@ + + + + + + + + + + + + + + + + + + + + + + From 968e82e5c3f7d3e5cb2effbe8c06e3a15734cfdf Mon Sep 17 00:00:00 2001 From: Jan Krieglstein Date: Thu, 19 Aug 2021 10:55:20 +0200 Subject: [PATCH 06/11] urdf: Rename jaw to finger --- ipa325_wsg50/urdf/wsg50.urdf.xacro | 26 ++++++++++++++------------ 1 file changed, 14 insertions(+), 12 deletions(-) diff --git a/ipa325_wsg50/urdf/wsg50.urdf.xacro b/ipa325_wsg50/urdf/wsg50.urdf.xacro index 3249c56..7ce1a91 100644 --- a/ipa325_wsg50/urdf/wsg50.urdf.xacro +++ b/ipa325_wsg50/urdf/wsg50.urdf.xacro @@ -32,33 +32,35 @@ - + - + - + - + - - + + + - + - + - - + + + - + - + From c7bf84a31388677227c49154a0fa9ec316351d2e Mon Sep 17 00:00:00 2001 From: Jan Krieglstein Date: Thu, 19 Aug 2021 10:54:12 +0200 Subject: [PATCH 07/11] Fix typo --- ipa325_wsg50/urdf/wsg50.urdf.xacro | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ipa325_wsg50/urdf/wsg50.urdf.xacro b/ipa325_wsg50/urdf/wsg50.urdf.xacro index 7ce1a91..484b226 100644 --- a/ipa325_wsg50/urdf/wsg50.urdf.xacro +++ b/ipa325_wsg50/urdf/wsg50.urdf.xacro @@ -3,7 +3,7 @@ - + From 018349b159bc1380a8f63a5e4ecd25bba3e08eb1 Mon Sep 17 00:00:00 2001 From: Jan Krieglstein Date: Wed, 11 Aug 2021 15:55:31 +0200 Subject: [PATCH 08/11] Do not publish the opening width as joint state --- ipa325_wsg50/src/WSG50ROSNode.cpp | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/ipa325_wsg50/src/WSG50ROSNode.cpp b/ipa325_wsg50/src/WSG50ROSNode.cpp index 9a54bbb..6874a54 100644 --- a/ipa325_wsg50/src/WSG50ROSNode.cpp +++ b/ipa325_wsg50/src/WSG50ROSNode.cpp @@ -477,10 +477,11 @@ void publishStates(const std::string &jointName, const std::string &openingJoint jointStateMsg.effort.push_back(systStateMsg.force/1000.0); //alternatively you can also use the full gripper opening - jointStateMsg.name.push_back(openingJointName); - jointStateMsg.position.push_back(systStateMsg.width/1000.0); - jointStateMsg.velocity.push_back(systStateMsg.speed/1000.0); - jointStateMsg.effort.push_back(systStateMsg.force/1000.0); + // should not be published as joint state, since there is no joint with this state. Use systStateMsg if you need full opening. + // jointStateMsg.name.push_back(openingJointName); + // jointStateMsg.position.push_back(systStateMsg.width/1000.0); + // jointStateMsg.velocity.push_back(systStateMsg.speed/1000.0); + // jointStateMsg.effort.push_back(systStateMsg.force/1000.0); jointStatePublisher.publish(jointStateMsg); From 6a6f4be14215b7ddd8b4030185078b44eb0ec3a6 Mon Sep 17 00:00:00 2001 From: Jan Krieglstein Date: Wed, 26 Apr 2023 14:44:11 +0200 Subject: [PATCH 09/11] Add hint that text based interface is required to be disabled --- README.md | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/README.md b/README.md index 74bb760..ba7e9cd 100644 --- a/README.md +++ b/README.md @@ -20,6 +20,10 @@ LIAA received funding from the European Union’s Seventh Framework Programme fo Project runtime: 02.09.2013 – 31.08.2017. +## Prerequisites + +On the WSG 50 Control panel (open WSG ip in browser, without https), go to `Settings/Command Interface` and ensure that "Use text based interface" is disabled. + ## ROS-Node Documentation Special Thanks to Mainauthor **Florian Röser** Current maintainer **Lorenz Halt** From f6932ebb33ee8ed99fe800804de72950522483df Mon Sep 17 00:00:00 2001 From: Jan Krieglstein Date: Wed, 26 Apr 2023 14:45:56 +0200 Subject: [PATCH 10/11] Introduce max_width parameter to support wsg50-68 and wsg50-210 --- ipa325_wsg50/include/WSG50Controller.h | 2 +- ipa325_wsg50/launch/wsg50-210.launch | 9 +++++++++ ipa325_wsg50/src/WSG50Controller.cpp | 16 ++++++++-------- ipa325_wsg50/src/WSG50ROSNode.cpp | 5 ++++- 4 files changed, 22 insertions(+), 10 deletions(-) create mode 100644 ipa325_wsg50/launch/wsg50-210.launch diff --git a/ipa325_wsg50/include/WSG50Controller.h b/ipa325_wsg50/include/WSG50Controller.h index e0f03e8..375ee99 100644 --- a/ipa325_wsg50/include/WSG50Controller.h +++ b/ipa325_wsg50/include/WSG50Controller.h @@ -100,7 +100,7 @@ class WSG50Controller : public WSG50Observer, WSG50RosSubject // Connection Manager // WSG50Controller(void); - WSG50Controller(std::string ip, std::string port); + WSG50Controller(std::string ip, std::string port, double max_width); virtual ~WSG50Controller(void); diff --git a/ipa325_wsg50/launch/wsg50-210.launch b/ipa325_wsg50/launch/wsg50-210.launch new file mode 100644 index 0000000..a2d3c04 --- /dev/null +++ b/ipa325_wsg50/launch/wsg50-210.launch @@ -0,0 +1,9 @@ + + + + + + + + + diff --git a/ipa325_wsg50/src/WSG50Controller.cpp b/ipa325_wsg50/src/WSG50Controller.cpp index 320c399..8218d54 100644 --- a/ipa325_wsg50/src/WSG50Controller.cpp +++ b/ipa325_wsg50/src/WSG50Controller.cpp @@ -18,9 +18,9 @@ * ### Define Specific Values for Schung WSG50-100 Gripper # * ############################################################ */ -#ifndef MAXWIDTH -#define MAXWIDTH 110.0 -#endif +// #ifndef MAXWIDTH +// #define MAXWIDTH 110.0 -> use parameter instead to allow using wsg50-210 as well +// #endif #ifndef MINWIDTH #define MINWIDTH 0.0 @@ -78,6 +78,7 @@ WSG50Controller::WSG50Controller(void) // this->_IP = "192.168.1.20"; this->_PORT = "1000"; + this->_MaxWidth = 110.0; // connect // @@ -101,11 +102,11 @@ WSG50Controller::~WSG50Controller(void) * string ip = IP of the gripper * string port = Port to connect with the gripper */ -WSG50Controller::WSG50Controller(std::string ip, std::string port) +WSG50Controller::WSG50Controller(std::string ip, std::string port, double max_width) { this->_IP = ip; this->_PORT = port; - + this->_MaxWidth = max_width; this->setupConnection(); } @@ -248,7 +249,6 @@ void WSG50Controller::setupConnection() // **************************************** // initialize max, min and default values // - this->_MaxWidth = MAXWIDTH; this->_MinWidth = MINWIDTH; this->_MaxSpeed = MAXSPEED; this->_MinSpeed = MINSPEED; @@ -257,7 +257,7 @@ void WSG50Controller::setupConnection() this->_MaxForceLimit = MAXFORCELIMIT; this->_MinForceLimit = MINFORCELIMIT; this->_softLimitMinus = MINWIDTH; - this->_softLimitPlus = MAXWIDTH; + this->_softLimitPlus = this->_MaxWidth; this->_acceleration = DEFAULTACCELERATION; // mm/s² this->_speed = DEFAULTSPEED; // mm/s this->_forceLimit = DEFAULTFORCELIMIT; // N @@ -541,7 +541,7 @@ void WSG50Controller::updateHandler(void) ROS_INFO("soft limits cleared successfully."); _softLimitsSet = false; _softLimitMinus = MINWIDTH; - _softLimitPlus = MAXWIDTH; + _softLimitPlus = this->_MaxWidth; } else if(resp.status_code == E_NO_PARAM_EXPECTED) { ROS_ERROR("CLEAR SOFT LIMITS response: No Parameter Expected!"); } else { diff --git a/ipa325_wsg50/src/WSG50ROSNode.cpp b/ipa325_wsg50/src/WSG50ROSNode.cpp index 6874a54..9fb9c0a 100644 --- a/ipa325_wsg50/src/WSG50ROSNode.cpp +++ b/ipa325_wsg50/src/WSG50ROSNode.cpp @@ -41,6 +41,7 @@ // Gripper Default Values // +#define MAXWIDTH 110.0 #define FORCELIMIT 10 // in Newton #define ACCELERATION 200 // in mm/s² @@ -637,13 +638,15 @@ int main(int argc, char** argv) // std::string ip, port; std::string jointName, openingJointName; + double max_width; ros::param::param("~ip", ip, DEFAULTIP); ros::param::param("~port", port, DEFAULTPORT); ros::param::param("~joint_name", jointName, DEFAULTJOINTNAME); ros::param::param("~opening_joint_name", openingJointName, DEFAULTOPENINGJOINTNAME); + ros::param::param("~max_width", max_width, MAXWIDTH); ROS_INFO_STREAM("Connecting to " << ip << ":" << port << "..."); try { - _controller = new WSG50Controller(ip, port); + _controller = new WSG50Controller(ip, port, max_width); } catch (int e) { ROS_ERROR("An exception occured while initializing the WSG50Controller!\nError: %d\nAborting further processing", e); return 0; From e1caa9cd0409c3cbf897e35f085cf7ab351e87a7 Mon Sep 17 00:00:00 2001 From: Jan Krieglstein Date: Wed, 28 Jun 2023 15:27:00 +0200 Subject: [PATCH 11/11] Remove code commented out --- ipa325_wsg50/src/WSG50ROSNode.cpp | 8 ++------ 1 file changed, 2 insertions(+), 6 deletions(-) diff --git a/ipa325_wsg50/src/WSG50ROSNode.cpp b/ipa325_wsg50/src/WSG50ROSNode.cpp index 9fb9c0a..177cbb0 100644 --- a/ipa325_wsg50/src/WSG50ROSNode.cpp +++ b/ipa325_wsg50/src/WSG50ROSNode.cpp @@ -477,12 +477,8 @@ void publishStates(const std::string &jointName, const std::string &openingJoint jointStateMsg.velocity.push_back(systStateMsg.speed/1000.0); jointStateMsg.effort.push_back(systStateMsg.force/1000.0); - //alternatively you can also use the full gripper opening - // should not be published as joint state, since there is no joint with this state. Use systStateMsg if you need full opening. - // jointStateMsg.name.push_back(openingJointName); - // jointStateMsg.position.push_back(systStateMsg.width/1000.0); - // jointStateMsg.velocity.push_back(systStateMsg.speed/1000.0); - // jointStateMsg.effort.push_back(systStateMsg.force/1000.0); + // the full gripper opening should not be published as joint state, since there is no joint with this state. + // Use systStateMsg if you need full opening. jointStatePublisher.publish(jointStateMsg);