Improvements, fixes

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**

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

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);
 
 

ipa325_wsg50/launch/wsg50-210.launch

@@ -0,0 +1,9 @@
+<?xml version="1.0"?>
+
+<launch>
+    <node name="ipa325_wsg50" pkg="ipa325_wsg50" type="schunk_gripper" output="screen">
+        <param name="ip" value="192.168.0.20"/>
+        <param name="port" value="1000"/>
+        <param name="max_width" value="210.0" />
+    </node>
+</launch>

ipa325_wsg50/package.xml

@@ -41,6 +41,7 @@
 
   <build_depend>roscpp</build_depend>
   <build_depend>std_msgs</build_depend>
+  <build_depend>std_srvs</build_depend>
   <build_depend>actionlib</build_depend>
   <build_depend>actionlib_msgs</build_depend>
   <build_depend>sensor_msgs</build_depend>
@@ -49,6 +50,7 @@
 
   <run_depend>roscpp</run_depend>
   <run_depend>std_msgs</run_depend>
+  <run_depend>std_srvs</run_depend>
   <run_depend>actionlib_msgs</run_depend>
   <run_depend>sensor_msgs</run_depend>
   <run_depend>trajectory_msgs</run_depend>

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;

ipa325_wsg50/src/WSG50Controller.cpp

@@ -10,17 +10,17 @@
  * ###   Define Default Values   #############
  * ###########################################
  */
-#define DEBUG true
+#define DEBUG false
 
 #define TIMEOUT     1       // sec
 
 /* ############################################################
  * ###   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
@@ -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
     //
@@ -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 {

ipa325_wsg50/src/WSG50ROSNode.cpp

@@ -29,11 +29,8 @@
 #include <ipa325_wsg50/setAcceleration.h>
 #include <ipa325_wsg50/setForceLimit.h>
 #include <ipa325_wsg50/setSoftLimits.h>
-#include <ipa325_wsg50/clearSoftLimits.h>
 #include <ipa325_wsg50/tareForceSensor.h>
-#include <ipa325_wsg50/stop.h>
-#include <ipa325_wsg50/fastStop.h>
-#include <ipa325_wsg50/ackFastStop.h>
+#include <std_srvs/Empty.h>
 
 // Programm Control
 //
@@ -44,6 +41,7 @@
 
 // Gripper Default Values
 //
+#define MAXWIDTH 110.0 
 #define FORCELIMIT  10          // in Newton
 #define ACCELERATION 200        // in mm/s²
 
@@ -309,6 +307,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);
@@ -479,10 +478,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);
 
@@ -543,8 +543,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");
 
@@ -575,8 +575,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");
 
@@ -592,8 +592,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");
 
@@ -607,8 +607,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");
 
@@ -638,13 +638,15 @@ int main(int argc, char** argv)
     //
     std::string ip, port;
     std::string jointName, openingJointName;
+    double max_width;
     ros::param::param<std::string>("~ip", ip, DEFAULTIP);
     ros::param::param<std::string>("~port", port, DEFAULTPORT);
     ros::param::param<std::string>("~joint_name", jointName, DEFAULTJOINTNAME);
     ros::param::param<std::string>("~opening_joint_name", openingJointName, DEFAULTOPENINGJOINTNAME);
+    ros::param::param<double>("~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;

ipa325_wsg50/src/lwr_testclient.cpp

@@ -23,11 +23,8 @@
 #include <ipa325_wsg50/setAcceleration.h>
 #include <ipa325_wsg50/setForceLimit.h>
 #include <ipa325_wsg50/setSoftLimits.h>
-#include <ipa325_wsg50/clearSoftLimits.h>
 #include <ipa325_wsg50/tareForceSensor.h>
-#include <ipa325_wsg50/stop.h>
-#include <ipa325_wsg50/fastStop.h>
-#include <ipa325_wsg50/ackFastStop.h>
+#include <std_srvs/Empty.h>
 
 // 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<ipa325_wsg50::clearSoftLimits>("ClearSoftLimits");
-    ipa325_wsg50::clearSoftLimits cslMsg;
+    ros::ServiceClient clearSoftLimitsClient = node.serviceClient<std_srvs::Empty>("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<ipa325_wsg50::stop>("Stop");
-            ipa325_wsg50::stop stopMsg;
+            ros::ServiceClient stopClient = node.serviceClient<std_srvs::Empty>("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<ipa325_wsg50::fastStop>("FastStop");
-            ipa325_wsg50::stop fastStopMsg;
+            ros::ServiceClient fastStopClient = node.serviceClient<std_srvs::Empty>("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<ipa325_wsg50::ackFastStop>("AcknowledgeFastStop");
-    ipa325_wsg50::ackFastStop ackMsg;
+    ros::ServiceClient ackFastStopClient = node.serviceClient<std_srvs::Empty>("AcknowledgeFastStop");
+    std_srvs::Empty ackMsg;
     ROS_INFO("Acknowledge Fast Stop");
     if(ackFastStopClient.call(ackMsg))
         ROS_INFO("acknowledgeFastStop successfully called!");