Communication mode with auto update by griiper; Readme; UDP/TCP

Author: nalt

.gitignore

@@ -3,3 +3,4 @@ srv_gen
 build
 bin
 ._*
+*.build

README.html

@@ -0,0 +1,52 @@
+<h1>wsg50-ros-pkg</h1>
+
+<p>Forked from: <a href="https://code.google.com/p/wsg50-ros-pkg">https://code.google.com/p/wsg50-ros-pkg</a></p>
+
+<p>See also Wiki: <a href="https://code.google.com/p/wsg50-ros-pkg/wiki/wsg_50">https://code.google.com/p/wsg50-ros-pkg/wiki/wsg_50</a></p>
+
+<h2>Node wsg_50_tcp</h2>
+
+<h3>Parameters</h3>
+
+<ul>
+<li><em>ip</em>: IP address of gripper</li>
+<li><em>port</em>: Port of gripper</li>
+<li><em>local_port</em>: Local port for UDP</li>
+<li><em>protocol</em>: udp or tcp (default)</li>
+<li><em>com_mode</em>: polling (default), script or auto_update. See communication modes below.</li>
+<li><em>rate</em>: Polling rate in Hz.</li>
+<li><em>grasping_force</em>: Set grasping force limit on startup</li>
+</ul>
+
+<h3>Services</h3>
+
+<p>See <a href="https://code.google.com/p/wsg50-ros-pkg/wiki/wsg_50">https://code.google.com/p/wsg50-ros-pkg/wiki/wsg_50</a>. Services currently block the reception of state updates.</p>
+
+<h3>Topics</h3>
+
+<ul>
+<li><em>~/goal_position [IN, wsg_50_common/Cmd], Modes: script, auto_update:</em> Position goal; send target position in mm and speed</li>
+<li><em>~/goal_speed [IN, std_msgs/Float32], Modes: script:</em> Velocity goal (in mm/s); positive values open the gripper</li>
+<li><em>~/moving [OUT, std_msgs/Bool], Modes: script, auto_update:</em> Signals a change in the motion state for position control. Can be used to wait for the end of a gripper movement. Signaling does not work correctly get for velocity control, since the gripper state register does not directly provide this information.</li>
+<li><em>~/state [OUT, std_msgs/State]:</em> State information (opening width, speed, forces). Note: Not all fields are available with all communication modes.</li>
+<li><em>/joint_states [OUT]:</em> Standard joint state message</li>
+</ul>
+
+<h3>Communication modes (closed-loop control)</h3>
+
+<p>Select by <em>com_mode</em> parameters.</p>
+
+<ul>
+<li><p><strong>Polling</strong><br />
+Gripper state is polled regularly using built-in commands. Services (e.g. move) block the polling as long as the gripper moves; the topic interface is not available. Up to 15 Hz could be reached with the WSG-50 hardware revision 2.</p></li>
+<li><p><strong>Script</strong><br />
+Allows for closed-loop control with a custom script (see below) that supports up to 2 FMF finger. Gripper state is read synchronously with the specified rate. Up to 30 Hz could be reached with the WSG-50 hardware revision 2. The gripper can be controlled asynchronously by sending position or velocity goals to the topics shown below. Commands will be sent with the next read command in the timer callback timer_cb().<br />
+The services can still be used - yet, they are blocking the gripper communication. There are no state updates while the gripper is moved by a service. </p></li>
+<li><p><strong>Auto_update</strong><br>
+Requests periodic updates of the gripper state (position, speed, force; less than with the script). Up to 140 Hz could be reached with the WSG-50 hardware revision 2. All responses of the gripper must be received by a reading thread which also publishes the ROS messages. Therefore, the commands in functions.cpp cannot be used. Position targets are sent asynchronously to the gripper using the built-in commands. As with the script option, there is a noticeable delay until the movement starts.<br />
+The services are disabled.</p></li>
+</ul>
+
+<h4>Gripper script</h4>
+
+<p>The script <em>cmd_measure.lua</em> must be running on the gripper for closed-loop control mode. It allows for non-blocking position and velocity control and responds with the current position, speed, motor force and up to to two FMF finger forces. The custom commands 0xB0 (read only), 0xB1 (read, goal position and speed), 0xB2 (read, goal speed) are used. Tested with firmware version 2.6.4. There have been minor API changes compared to 1.x.</p>

README.md

@@ -0,0 +1,44 @@
+# wsg50-ros-pkg
+Forked from: [https://code.google.com/p/wsg50-ros-pkg](https://code.google.com/p/wsg50-ros-pkg)
+
+See also Wiki: [https://code.google.com/p/wsg50-ros-pkg/wiki/wsg_50](https://code.google.com/p/wsg50-ros-pkg/wiki/wsg_50)
+
+## Node wsg\_50_tcp
+
+### Parameters
+* *ip*: IP address of gripper
+* *port*: Port of gripper
+* *local_port*: Local port for UDP
+* *protocol*: udp or tcp (default)
+* *com_mode*: polling (default), script or auto_update. See communication modes below.
+* *rate*: Polling rate in Hz.
+* *grasping_force*: Set grasping force limit on startup
+
+
+### Services
+See [https://code.google.com/p/wsg50-ros-pkg/wiki/wsg_50](https://code.google.com/p/wsg50-ros-pkg/wiki/wsg_50). Services currently block the reception of state updates.
+
+### Topics
+* *~/goal\_position [IN, wsg_50_common/Cmd], Modes: script, auto_update:* Position goal; send target position in mm and speed
+* *~/goal\_speed [IN, std_msgs/Float32], Modes: script:* Velocity goal (in mm/s); positive values open the gripper
+* *~/moving [OUT, std_msgs/Bool], Modes: script, auto_update:* Signals a change in the motion state for position control. Can be used to wait for the end of a gripper movement. Signaling does not work correctly get for velocity control, since the gripper state register does not directly provide this information.
+* *~/state [OUT, std_msgs/State]:* State information (opening width, speed, forces). Note: Not all fields are available with all communication modes.
+* */joint_states [OUT]:* Standard joint state message
+
+
+### Communication modes (closed-loop control)
+Select by *com_mode* parameters.
+
+* **Polling**<br />
+Gripper state is polled regularly using built-in commands. Services (e.g. move) block the polling as long as the gripper moves; the topic interface is not available. Up to 15 Hz could be reached with the WSG-50 hardware revision 2.
+
+* **Script**<br />
+Allows for closed-loop control with a custom script (see below) that supports up to 2 FMF finger. Gripper state is read synchronously with the specified rate. Up to 30 Hz could be reached with the WSG-50 hardware revision 2. The gripper can be controlled asynchronously by sending position or velocity goals to the topics shown below. Commands will be sent with the next read command in the timer callback timer_cb().<br />
+The services can still be used - yet, they are blocking the gripper communication. There are no state updates while the gripper is moved by a service. 
+
+* **Auto_update**<br>
+Requests periodic updates of the gripper state (position, speed, force; less than with the script). Up to 140 Hz could be reached with the WSG-50 hardware revision 2. All responses of the gripper must be received by a reading thread which also publishes the ROS messages. Therefore, the commands in functions.cpp cannot be used. Position targets are sent asynchronously to the gripper using the built-in commands. As with the script option, there is a noticeable delay until the movement starts.<br />
+The services are disabled.
+
+#### Gripper script
+The script *cmd_measure.lua* must be running on the gripper for the script mode. It allows for non-blocking position and velocity control and responds with the current position, speed, motor force and up to two FMF finger forces. The custom commands 0xB0 (read only), 0xB1 (read, goal position and speed), 0xB2 (read, goal speed) are used. Tested with firmware version 2.6.4. There have been minor API changes compared to 1.x.

wsg_50/CMakeLists.txt

@@ -33,16 +33,16 @@ rosbuild_gensrv()
 # WSG_50_TCP version
 
 set(DRIVER_SOURCES 
-  src/checksum.cpp
-  src/cmd.c
-  src/common.cpp
-  src/functions.cpp
-  src/interface.cpp
+  src/checksum.cpp include/wsg_50/checksum.h
+  src/cmd.c include/wsg_50/cmd.h
+  src/common.cpp include/wsg_50/common.h
+  src/functions.cpp include/wsg_50/functions.h
+  src/interface.cpp include/wsg_50/interface.h
   src/main.cpp
-  src/msg.c
-  src/serial.c
-  src/tcp.c
-  src/udp.c)
+  src/msg.c include/wsg_50/msg.h
+  src/serial.c include/wsg_50/serial.h
+  src/tcp.c include/wsg_50/tcp.h
+  src/udp.c include/wsg_50/udp.h)
 
 
 rosbuild_add_executable(wsg_50_tcp src/main.cpp ${DRIVER_SOURCES})
@@ -59,13 +59,12 @@ set(DRIVER_SOURCES_CAN
 #rosbuild_add_executable(wsg_50_can src/main_can.cpp ${DRIVER_SOURCES_CAN})
 
 add_definitions(-DOSNAME_LINUX)
-add_definitions(-std=c++0x)
-
-include_directories(/home/marc/peak-linux-driver-7.5/driver/src/)
-link_directories(/home/marc/peak-linux-driver-7.5/lib/)
+add_definitions(-g)
+set(CMAKE_CXX_FLAGS ${CMAKE_CXX_FLAGS} "-std=c++0x") # Need C++11
 
+#include_directories(/home/marc/peak-linux-driver-7.5/driver/src/)
+#link_directories(/home/marc/peak-linux-driver-7.5/lib/)
 #rosbuild_add_compile_flags(wsg_50_can -g -Wall)
-
 #target_link_libraries(wsg_50_can pcan)
 
 

wsg_50/include/wsg_50/functions.h

@@ -65,9 +65,10 @@ typedef struct {
 // Function declaration
 //------------------------------------------------------------------------
 
+float convert(unsigned char *b);
 int homing( void );
-int move( float width, float speed, bool stop_on_block );
-int stop( void );
+int move(float width, float speed, bool stop_on_block, bool ignore_response = false);
+int stop( bool ignore_response = false );
 int grasp( float objWidth, float speed );
 int release( float width, float speed );
 int ack_fault( void );
@@ -77,12 +78,13 @@ int setGraspingForceLimit( float force );
 
 const char * systemState( void );
 int graspingState( void );
-float getOpening( void );
-float getForce( void );
+float getOpening(int auto_update = 0);
+float getForce(int auto_update = 0);
+float getSpeed(int auto_update = 0);
 int getAcceleration( void );
 int getGraspingForceLimit( void );
 
-int measure_move (unsigned char cmd_type, float cmd_width, float cmd_speed, gripper_response & info);
+int script_measure_move (unsigned char cmd_type, float cmd_width, float cmd_speed, gripper_response & info);
 
 //void getStateValues(); //(unsigned char *);
 

wsg_50/launch/wsg_50_tcp_script.launch

@@ -1,10 +1,12 @@
 <launch>
+  <arg name="com_mode" default="script" /><!-- or auto_update, polling -->
 
   <node  name="wsg_50"  pkg="wsg_50" type="wsg_50_tcp" output="screen">
 	<param name="ip" type="string" value="192.168.20.2"/>
 	<param name="port" type="int" value="1000"/>
-    <param name="use_script" type="bool" value="true"/>
-    <param name="rate" type="double" value="50"/> <!-- WSG50 HW revision 2: up to 30 Hz -->
+    <param name="protocol" type="string" value="tcp"/>
+    <param name="com_mode" type="string" value="$(arg com_mode)"/>
+    <param name="rate" type="double" value="500"/> <!-- WSG50 HW revision 2: up to 30 Hz with script; 130Hz with auto_update -->
     <param name="grasping_force" type="double" value="80"/>
   </node>
 

wsg_50/launch/wsg_50_udp_script.launch

@@ -0,0 +1,15 @@
+<launch>
+  <arg name="com_mode" default="script" />
+
+  <node  name="wsg_50"  pkg="wsg_50" type="wsg_50_tcp" output="screen">
+	<param name="ip" type="string" value="192.168.20.2"/>
+	<param name="port" type="int" value="1500"/>
+	<param name="local_port" type="int" value="5501"/>
+    <param name="protocol" type="string" value="udp"/>
+    <param name="com_mode" type="string" value="$(arg com_mode)"/>
+    <param name="rate" type="double" value="500"/>
+    <param name="grasping_force" type="double" value="80"/>
+  </node>
+
+</launch>
+

wsg_50/src/common.cpp

@@ -290,9 +290,6 @@ const char * getStateValues( unsigned char *b ){
 
 	//dbgPrint("%s\n", resp);
 	return resp;
-
-
-
 }