├── .gitignore
├── README.html
├── README.md
├── wsg_50_common
    ├── CMakeLists.txt
    ├── msg
    │   ├── Cmd.msg
    │   └── Status.msg
    ├── package.xml
    └── srv
    │   ├── Conf.srv
    │   ├── Incr.srv
    │   └── Move.srv
├── wsg_50_driver
    ├── CMakeLists.txt
    ├── cmd_measure.lua
    ├── include
    │   └── wsg_50
    │   │   ├── aux_.h
    │   │   ├── char.h
    │   │   ├── checksum.h
    │   │   ├── cmd.h
    │   │   ├── common.h
    │   │   ├── functions.h
    │   │   ├── functions_can.h
    │   │   ├── interface.h
    │   │   ├── msg.h
    │   │   ├── serial.h
    │   │   ├── tcp.h
    │   │   └── udp.h
    ├── launch
    │   ├── wsg_50_can.launch
    │   ├── wsg_50_tcp.launch
    │   ├── wsg_50_tcp_script.launch
    │   └── wsg_50_udp_script.launch
    ├── package.xml
    └── src
    │   ├── checksum.cpp
    │   ├── cmd.c
    │   ├── common.cpp
    │   ├── functions.cpp
    │   ├── functions_can.cpp
    │   ├── interface.cpp
    │   ├── main.cpp
    │   ├── main_can.cpp
    │   ├── msg.c
    │   ├── serial.c
    │   ├── tcp.c
    │   └── udp.c
└── wsg_50_simulation
    ├── CMakeLists.txt
    ├── controllers
        ├── wsg_50_gl.yaml
        ├── wsg_50_gl_modular.yaml
        ├── wsg_50_gl_powerball.yaml
        ├── wsg_50_gr.yaml
        ├── wsg_50_gr_modular.yaml
        └── wsg_50_gr_powerball.yaml
    ├── launch
        ├── wsg_50.launch
        ├── wsg_50_controllers.launch
        ├── wsg_50_modular_controllers.launch
        ├── wsg_50_powerball_controllers.launch
        └── wsg_50_teleop.launch
    ├── meshes
        ├── GUIDE_WSG50_110.stl
        ├── WSG-FMF.stl
        └── WSG50_110.stl
    ├── package.xml
    ├── src
        ├── wsg_50_keyboard_teleop.cpp
        └── wsg_50_sim_driver.cpp
    ├── urdf
        ├── wsg_50.urdf
        ├── wsg_50.urdf.xacro
        └── wsg_50_dependent_joints.yaml
    ├── wsg_50_gl.yaml
    └── wsg_50_gr.yaml


/.gitignore:
--------------------------------------------------------------------------------
1 | msg_gen
2 | srv_gen
3 | build
4 | bin
5 | ._*
6 | *.build
7 | 


--------------------------------------------------------------------------------
/README.html:
--------------------------------------------------------------------------------
 1 | <h1>ROS package for Schunk WSG-50 Gripper</h1>
 2 | 
 3 | <p>Forked from: <a href="https://code.google.com/p/wsg50-ros-pkg">https://code.google.com/p/wsg50-ros-pkg</a></p>
 4 | 
 5 | <p>Modifications of this repository:
 6 | Reading back state with high rates, open-loop control via topics, catkinized, modifications for hydro.
 7 | Existing features are not discussed here - see original 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>
 8 | 
 9 | <p>Todo: Restructure code</p>
10 | 
11 | <h2>Node wsg_50_ip (was: wsg_50_tcp)</h2>
12 | 
13 | <h3>Parameters</h3>
14 | 
15 | <ul>
16 | <li><em>ip</em>: IP address of gripper</li>
17 | <li><em>port</em>: Port of gripper</li>
18 | <li><em>local_port</em>: Local port for UDP</li>
19 | <li><em>protocol</em>: udp or tcp (default)</li>
20 | <li><em>com_mode</em>: polling (default), script or auto_update. See communication modes below.</li>
21 | <li><em>rate</em>: Polling rate in Hz.</li>
22 | <li><em>grasping_force</em>: Set grasping force limit on startup</li>
23 | </ul>
24 | 
25 | <h3>Services</h3>
26 | 
27 | <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>
28 | 
29 | <h3>Topics</h3>
30 | 
31 | <ul>
32 | <li><em>~/goal_position [IN, wsg_50_common/Cmd]</em>, in modes script, auto_update:<br/>
33 | Position goal; send target position in mm and speed</li>
34 | <li><em>~/goal_speed [IN, std_msgs/Float32]</em>, in mode script:<br/>
35 | Velocity goal (in mm/s); positive values open the gripper</li>
36 | <li><em>~/moving [OUT, std_msgs/Bool]</em>, in modes script, auto_update:<br/>
37 | Signals a change in the motion state for position control. Can be used to wait for the end of a gripper movement. Does not work correctly yet for velocity control, since the gripper state register does not directly provide this information.</li>
38 | <li><em>~/state [OUT, std_msgs/State]:</em><br/>
39 | State information (opening width, speed, forces). Note: Not all fields are available with all communication modes.</li>
40 | <li><em>/joint_states [OUT, sensor_msgs/JointState]:</em><br/>
41 | Standard joint state message</li>
42 | </ul>
43 | 
44 | <h3>Communication modes (closed-loop control)</h3>
45 | 
46 | <p>Select by <em>com_mode</em> parameter.</p>
47 | 
48 | <ul>
49 | <li><p><strong>Polling</strong><br />
50 | Gripper state is polled regularly using built-in commands (original implementaion). Service calls (e.g. move) block 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>
51 | <li><p><strong>Script</strong><br />
52 | 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 listed above. Commands will be sent with the next read command in the timer callback timer_cb().<br />
53 | The service interface 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>
54 | <li><p><strong>Auto_update</strong><br>
55 | Requests periodic updates of the gripper state (position, speed, force; less data 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, most commands in functions.cpp cannot be used. Position targets are sent asynchronously to the gripper using the built-in commands.<br />
56 | The services are disabled.</p></li>
57 | </ul>
58 | 
59 | <h4>Gripper script</h4>
60 | 
61 | <p>The script <em>cmd_measure.lua</em> 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 since 1.x.</p>
62 | 
63 | <h2>Node wsg_50_can</h2>
64 | 
65 | <p>Remains unchanged; new features not implemented here. </p>
66 | 


--------------------------------------------------------------------------------
/README.md:
--------------------------------------------------------------------------------
 1 | # ROS package for Schunk WSG-50 Gripper
 2 | Forked from: [https://code.google.com/p/wsg50-ros-pkg](https://code.google.com/p/wsg50-ros-pkg)
 3 | 
 4 | Modifications of this repository:
 5 | Reading back state with high rates, open-loop control via topics, catkinized, modifications for hydro.
 6 | Existing features are not discussed here - see original Wiki: [https://code.google.com/p/wsg50-ros-pkg/wiki/wsg_50](https://code.google.com/p/wsg50-ros-pkg/wiki/wsg_50)
 7 | 
 8 | Todo: Restructure code
 9 | 
10 | 
11 | ## Node wsg\_50\_ip (was: wsg\_50_tcp)
12 | 
13 | ### Parameters
14 | * *ip*: IP address of gripper
15 | * *port*: Port of gripper
16 | * *local_port*: Local port for UDP
17 | * *protocol*: udp or tcp (default)
18 | * *com_mode*: polling (default), script or auto_update. See communication modes below.
19 | * *rate*: Polling rate in Hz.
20 | * *grasping_force*: Set grasping force limit on startup
21 | 
22 | 
23 | ### Services
24 | 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.
25 | 
26 | ### Topics
27 | * *~/goal\_position [IN, wsg_50_common/Cmd]*, in modes script, auto_update:<br/>
28 | Position goal; send target position in mm and speed
29 | * *~/goal\_speed [IN, std_msgs/Float32]*, in mode script:<br/>
30 | Velocity goal (in mm/s); positive values open the gripper
31 | * *~/moving [OUT, std_msgs/Bool]*, in modes script, auto_update:<br/>
32 | Signals a change in the motion state for position control. Can be used to wait for the end of a gripper movement. Does not work correctly yet for velocity control, since the gripper state register does not directly provide this information.
33 | * *~/state [OUT, std_msgs/State]:*<br/>
34 | State information (opening width, speed, forces). Note: Not all fields are available with all communication modes.
35 | * */joint_states [OUT, sensor_msgs/JointState]:*<br/>
36 | Standard joint state message
37 | 
38 | 
39 | ### Communication modes (closed-loop control)
40 | Select by *com_mode* parameter.
41 | 
42 | * **Polling**<br />
43 | Gripper state is polled regularly using built-in commands (original implementaion). Service calls (e.g. move) block 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.
44 | 
45 | * **Script**<br />
46 | 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 listed above. Commands will be sent with the next read command in the timer callback timer_cb().<br />
47 | The service interface can still be used - yet, they are blocking the gripper communication. There are no state updates while the gripper is moved by a service. 
48 | 
49 | * **Auto_update**<br>
50 | Requests periodic updates of the gripper state (position, speed, force; less data 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, most commands in functions.cpp cannot be used. Position targets are sent asynchronously to the gripper using the built-in commands.<br />
51 | The services are disabled.
52 | 
53 | #### Gripper script
54 | 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 since 1.x.
55 | 
56 | 
57 | ## Node wsg\_50_can
58 | 
59 | Remains unchanged; new features not implemented here. 


--------------------------------------------------------------------------------
/wsg_50_common/CMakeLists.txt:
--------------------------------------------------------------------------------
  1 | cmake_minimum_required(VERSION 2.8.3)
  2 | project(wsg_50_common)
  3 | 
  4 | ## Find catkin macros and libraries
  5 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
  6 | ## is used, also find other catkin packages
  7 | find_package(catkin REQUIRED COMPONENTS
  8 |   message_generation
  9 |   std_msgs
 10 | )
 11 | 
 12 | ## System dependencies are found with CMake's conventions
 13 | # find_package(Boost REQUIRED COMPONENTS system)
 14 | 
 15 | 
 16 | ## Uncomment this if the package has a setup.py. This macro ensures
 17 | ## modules and global scripts declared therein get installed
 18 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
 19 | # catkin_python_setup()
 20 | 
 21 | ################################################
 22 | ## Declare ROS messages, services and actions ##
 23 | ################################################
 24 | 
 25 | ## To declare and build messages, services or actions from within this
 26 | ## package, follow these steps:
 27 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in
 28 | ##   your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
 29 | ## * In the file package.xml:
 30 | ##   * add a build_depend and a run_depend tag for each package in MSG_DEP_SET
 31 | ##   * If MSG_DEP_SET isn't empty the following dependencies might have been
 32 | ##     pulled in transitively but can be declared for certainty nonetheless:
 33 | ##     * add a build_depend tag for "message_generation"
 34 | ##     * add a run_depend tag for "message_runtime"
 35 | ## * In this file (CMakeLists.txt):
 36 | ##   * add "message_generation" and every package in MSG_DEP_SET to
 37 | ##     find_package(catkin REQUIRED COMPONENTS ...)
 38 | ##   * add "message_runtime" and every package in MSG_DEP_SET to
 39 | ##     catkin_package(CATKIN_DEPENDS ...)
 40 | ##   * uncomment the add_*_files sections below as needed
 41 | ##     and list every .msg/.srv/.action file to be processed
 42 | ##   * uncomment the generate_messages entry below
 43 | ##   * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
 44 | 
 45 | ## Generate messages in the 'msg' folder
 46 | add_message_files(FILES Cmd.msg Status.msg)
 47 | 
 48 | ## Generate services in the 'srv' folder
 49 | add_service_files(FILES Conf.srv Incr.srv Move.srv)
 50 | 
 51 | ## Generate actions in the 'action' folder
 52 | # add_action_files(
 53 | #   FILES
 54 | #   Action1.action
 55 | #   Action2.action
 56 | # )
 57 | 
 58 | ## Generate added messages and services with any dependencies listed here
 59 | generate_messages(DEPENDENCIES std_msgs)
 60 | 
 61 | ###################################
 62 | ## catkin specific configuration ##
 63 | ###################################
 64 | ## The catkin_package macro generates cmake config files for your package
 65 | ## Declare things to be passed to dependent projects
 66 | ## INCLUDE_DIRS: uncomment this if you package contains header files
 67 | ## LIBRARIES: libraries you create in this project that dependent projects also need
 68 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need
 69 | ## DEPENDS: system dependencies of this project that dependent projects also need
 70 | catkin_package(
 71 | #  INCLUDE_DIRS include
 72 | #  LIBRARIES wsg_50_common
 73 |   CATKIN_DEPENDS message_runtime std_msgs
 74 | #  DEPENDS system_lib
 75 | )
 76 | 
 77 | ###########
 78 | ## Build ##
 79 | ###########
 80 | 
 81 | ## Specify additional locations of header files
 82 | ## Your package locations should be listed before other locations
 83 | # include_directories(include)
 84 | include_directories(
 85 |   ${catkin_INCLUDE_DIRS}
 86 | )
 87 | 
 88 | ## Declare a cpp library
 89 | # add_library(wsg_50_common
 90 | #   src/${PROJECT_NAME}/wsg_50_common.cpp
 91 | # )
 92 | 
 93 | ## Declare a cpp executable
 94 | # add_executable(wsg_50_common_node src/wsg_50_common_node.cpp)
 95 | 
 96 | ## Add cmake target dependencies of the executable/library
 97 | ## as an example, message headers may need to be generated before nodes
 98 | # add_dependencies(wsg_50_common_node wsg_50_common_generate_messages_cpp)
 99 | 
100 | ## Specify libraries to link a library or executable target against
101 | # target_link_libraries(wsg_50_common_node
102 | #   ${catkin_LIBRARIES}
103 | # )
104 | 
105 | #############
106 | ## Install ##
107 | #############
108 | 
109 | # all install targets should use catkin DESTINATION variables
110 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
111 | 
112 | ## Mark executable scripts (Python etc.) for installation
113 | ## in contrast to setup.py, you can choose the destination
114 | # install(PROGRAMS
115 | #   scripts/my_python_script
116 | #   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
117 | # )
118 | 
119 | ## Mark executables and/or libraries for installation
120 | # install(TARGETS wsg_50_common wsg_50_common_node
121 | #   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
122 | #   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
123 | #   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
124 | # )
125 | 
126 | ## Mark cpp header files for installation
127 | # install(DIRECTORY include/${PROJECT_NAME}/
128 | #   DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
129 | #   FILES_MATCHING PATTERN "*.h"
130 | #   PATTERN ".svn" EXCLUDE
131 | # )
132 | 
133 | ## Mark other files for installation (e.g. launch and bag files, etc.)
134 | # install(FILES
135 | #   # myfile1
136 | #   # myfile2
137 | #   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
138 | # )
139 | 
140 | #############
141 | ## Testing ##
142 | #############
143 | 
144 | ## Add gtest based cpp test target and link libraries
145 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_wsg_50_common.cpp)
146 | # if(TARGET ${PROJECT_NAME}-test)
147 | #   target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
148 | # endif()
149 | 
150 | ## Add folders to be run by python nosetests
151 | # catkin_add_nosetests(test)
152 | 


--------------------------------------------------------------------------------
/wsg_50_common/msg/Cmd.msg:
--------------------------------------------------------------------------------
1 | string mode
2 | float32 pos
3 | float32 speed
4 | 
5 | 


--------------------------------------------------------------------------------
/wsg_50_common/msg/Status.msg:
--------------------------------------------------------------------------------
1 | string status
2 | float32 width
3 | float32 speed
4 | float32 acc
5 | float32 force
6 | float32 force_finger0
7 | float32 force_finger1
8 | 


--------------------------------------------------------------------------------
/wsg_50_common/package.xml:
--------------------------------------------------------------------------------
 1 | <?xml version="1.0"?>
 2 | <package>
 3 |   <name>wsg_50_common</name>
 4 |   <version>0.0.0</version>
 5 |   <description>The wsg_50_common package</description>
 6 | 
 7 |   <!-- One maintainer tag required, multiple allowed, one person per tag --> 
 8 |   <!-- Example:  -->
 9 |   <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
10 |   <maintainer email="nalt@todo.todo">nalt</maintainer>
11 | 
12 | 
13 |   <!-- One license tag required, multiple allowed, one license per tag -->
14 |   <!-- Commonly used license strings: -->
15 |   <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
16 |   <license>BSD</license>
17 | 
18 | 
19 |   <!-- Url tags are optional, but mutiple are allowed, one per tag -->
20 |   <!-- Optional attribute type can be: website, bugtracker, or repository -->
21 |   <!-- Example: -->
22 |   <!-- <url type="website">http://wiki.ros.org/wsg_50_common</url> -->
23 | 
24 | 
25 |   <!-- Author tags are optional, mutiple are allowed, one per tag -->
26 |   <!-- Authors do not have to be maintianers, but could be -->
27 |   <!-- Example: -->
28 |   <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
29 |   <author >Nicolas Alt</author>
30 |   <author >Robotnik</author>
31 |   <author >Weiss Robotics</author>
32 | 
33 | 
34 |   <!-- The *_depend tags are used to specify dependencies -->
35 |   <!-- Dependencies can be catkin packages or system dependencies -->
36 |   <!-- Examples: -->
37 |   <!-- Use build_depend for packages you need at compile time: -->
38 |   <!--   <build_depend>message_generation</build_depend> -->
39 |   <!-- Use buildtool_depend for build tool packages: -->
40 |   <!--   <buildtool_depend>catkin</buildtool_depend> -->
41 |   <!-- Use run_depend for packages you need at runtime: -->
42 |   <!--   <run_depend>message_runtime</run_depend> -->
43 |   <!-- Use test_depend for packages you need only for testing: -->
44 |   <!--   <test_depend>gtest</test_depend> -->
45 |   <buildtool_depend>catkin</buildtool_depend>
46 |   <build_depend>message_generation</build_depend>
47 |   <build_depend>std_msgs</build_depend>
48 |   <run_depend>message_runtime</run_depend>
49 |   <run_depend>std_msgs</run_depend>
50 | 
51 | 
52 |   <!-- The export tag contains other, unspecified, tags -->
53 |   <export>
54 |     <!-- You can specify that this package is a metapackage here: -->
55 |     <!-- <metapackage/> -->
56 | 
57 |     <!-- Other tools can request additional information be placed here -->
58 | 
59 |   </export>
60 | </package>


--------------------------------------------------------------------------------
/wsg_50_common/srv/Conf.srv:
--------------------------------------------------------------------------------
1 | float32 val
2 | ---
3 | uint8 error
4 | 


--------------------------------------------------------------------------------
/wsg_50_common/srv/Incr.srv:
--------------------------------------------------------------------------------
1 | string direction
2 | float32 increment
3 | ---
4 | uint8 error
5 | 


--------------------------------------------------------------------------------
/wsg_50_common/srv/Move.srv:
--------------------------------------------------------------------------------
1 | float32 width
2 | float32 speed
3 | ---
4 | uint8 error
5 | 


--------------------------------------------------------------------------------
/wsg_50_driver/CMakeLists.txt:
--------------------------------------------------------------------------------
 1 | cmake_minimum_required(VERSION 2.8.3)
 2 | project(wsg_50_driver)
 3 | 
 4 | ## Find catkin macros and libraries
 5 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
 6 | ## is used, also find other catkin packages
 7 | find_package(catkin REQUIRED COMPONENTS
 8 |   roscpp
 9 |   std_msgs
10 |   std_srvs
11 |   wsg_50_common
12 | )
13 | 
14 | catkin_package(
15 | #   INCLUDE_DIRS include
16 | #  LIBRARIES vh_pixelmap
17 |   CATKIN_DEPENDS roscpp std_msgs std_srvs wsg_50_common
18 | #  DEPENDS system_lib
19 | )
20 | 
21 | ## System dependencies are found with CMake's conventions
22 | # find_package(Boost REQUIRED COMPONENTS system)
23 | 
24 | 
25 | # WSG_50_TCP version
26 | set(DRIVER_SOURCES
27 |   src/checksum.cpp include/wsg_50/checksum.h
28 |   src/cmd.c include/wsg_50/cmd.h
29 |   src/common.cpp include/wsg_50/common.h
30 |   src/functions.cpp include/wsg_50/functions.h
31 |   src/interface.cpp include/wsg_50/interface.h
32 |   src/main.cpp
33 |   src/msg.c include/wsg_50/msg.h
34 |   src/serial.c include/wsg_50/serial.h
35 |   src/tcp.c include/wsg_50/tcp.h
36 |   src/udp.c include/wsg_50/udp.h)
37 | 
38 | # WSG_50_CAN version
39 | set(DRIVER_SOURCES_CAN
40 |   src/checksum.cpp
41 |   src/common.cpp
42 |   src/functions_can.cpp
43 |   src/msg.c)
44 | 
45 | include_directories(
46 |   include
47 |   ${catkin_INCLUDE_DIRS}
48 | )
49 | 
50 | add_definitions(-DOSNAME_LINUX)
51 | add_definitions(-g)
52 | 
53 | # C++11 Needed
54 | include(CheckCXXCompilerFlag)
55 | 
56 | CHECK_CXX_COMPILER_FLAG("-std=c++11" COMPILER_SUPPORTS_CXX11)
57 | CHECK_CXX_COMPILER_FLAG("-std=c++0x" COMPILER_SUPPORTS_CXX0X)
58 | 
59 | if(COMPILER_SUPPORTS_CXX11)
60 |     set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11")
61 | elseif(COMPILER_SUPPORTS_CXX0X)
62 |     set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x")
63 | else()
64 |     message(FATAL_ERROR "The compiler ${CMAKE_CXX_COMPILER} has no C++11 support. Please use a different C++ compiler.")
65 | endif()
66 | 
67 | add_executable(wsg_50_ip src/main.cpp ${DRIVER_SOURCES})
68 | target_link_libraries(wsg_50_ip ${catkin_LIBRARIES})
69 | add_dependencies(wsg_50_ip wsg_50_common_generate_messages_cpp)
70 | 
71 | #add_executable(wsg_50_can src/main_can.cpp src/checksum.cpp src/msg.c src/common.cpp src/functions_can.cpp)
72 | #add_executable(wsg_50_can src/main_can.cpp ${DRIVER_SOURCES_CAN})
73 | 
74 | #include_directories(/home/marc/peak-linux-driver-7.5/driver/src/)
75 | #link_directories(/home/marc/peak-linux-driver-7.5/lib/)
76 | #add_compile_flags(wsg_50_can -g -Wall)
77 | #target_link_libraries(wsg_50_can pcan)
78 | 


--------------------------------------------------------------------------------
/wsg_50_driver/cmd_measure.lua:
--------------------------------------------------------------------------------
 1 | -- Nicolas Alt, 2014-09-04
 2 | -- Command-and-measure script
 3 | -- Works with extended wsg_50 ROS package
 4 | -- Tests showed about 20Hz rate
 5 | 
 6 | cmd.register(0xB0); -- Measure only
 7 | cmd.register(0xB1); -- Position control
 8 | cmd.register(0xB2); -- Speed control
 9 | def_speed = 5;
10 | is_speed = false;
11 | 
12 | -- Get number of FMF fingers
13 | nfin = 0;
14 | for i = 0,1 do
15 |     if finger.type(i) == "fmf" then
16 |         nfin = nfin + 1;
17 |     else
18 |         break;
19 |     end
20 | end
21 | printf("#FMF fingers: %d\n", nfin)
22 | 
23 | function hasbit(x, p)
24 |   return x % (p + p) >= p       
25 | end
26 | 
27 | function process()
28 |     id, payload = cmd.read();
29 |     -- ==== Measurements (1) ====
30 |     busy = mc.busy()
31 |     blocked = mc.blocked()
32 |     pos = mc.position();
33 |     
34 |     -- Position control
35 |     if id == 0xB1 then
36 |         cmd_width = bton({payload[2],payload[3],payload[4],payload[5]});
37 |         cmd_speed = bton({payload[6],payload[7],payload[8],payload[9]});
38 |         -- printf("Got command %f, %f\n", cmd_width, cmd_speed)
39 |         print("set_pos");
40 |         if busy then mc.stop(); end
41 |         mc.move(cmd_width, math.abs(cmd_speed), 0)
42 |     -- Velocity control
43 |     elseif id == 0xB2 then
44 |         -- do_speed = hasbit(payload[1], 0x02);
45 |         cmd_speed = bton({payload[6],payload[7],payload[8],payload[9]});
46 |         print("set_speed");
47 |         is_speed = true;
48 |         def_speed = cmd_speed;
49 |         mc.speed(cmd_speed);
50 |     end
51 |         
52 |        
53 |     -- ==== Actions ====
54 |     -- Stop if in speed mode
55 |     -- print(blocked, is_speed, pos);
56 |     if blocked and is_speed and pos <= 50 and def_speed < 0 then
57 |         print("stop");
58 |         mc.stop(); is_speed = false;
59 |     end
60 |     if blocked and is_speed and pos >= 50 and def_speed > 0 then
61 |         print("stop"); 
62 |         mc.stop(); is_speed = false;
63 |     end           
64 |     
65 |     -- ==== Get measurements ====
66 |     state = gripper.state();
67 |     busy = mc.busy();
68 |     blocked = mc.blocked();
69 |     pos = mc.position();
70 |     speed = mc.speed();
71 |     force = mc.aforce();
72 | 
73 |     force_l = math.nan; force_r = math.nan;
74 |     if nfin >= 1 then force_l = finger.data(0) end
75 |     if nfin >= 2 then force_r = finger.data(1) end
76 |     
77 |     if cmd.online() then
78 |         -- Only the lowest byte of state is sent!
79 |         cmd.send(id, etob(E_SUCCESS), state % 256,
80 |             { ntob(pos), ntob(speed), ntob(force), ntob(force_l), ntob(force_r)});
81 |     end
82 | end
83 | 
84 | while true do
85 |     if cmd.online() then
86 |         -- process()
87 |         if not pcall(process) then
88 |             print("Error occured")
89 |             sleep(100)
90 |         end
91 |     else
92 |         sleep(100)
93 |     end
94 | end


--------------------------------------------------------------------------------
/wsg_50_driver/include/wsg_50/aux_.h:
--------------------------------------------------------------------------------
 1 | #include <libpcan.h>
 2 | #include <pcan.h>
 3 | 
 4 | #define DEFAULT_NODE "/dev/pcan32"
 5 | 
 6 | HANDLE h;
 7 | 
 8 | TPCANMsg msg;
 9 | //TPCANRdMsg *tpcmsg;
10 | unsigned short crc;
11 | unsigned char header[6]; 
12 | unsigned char data[1];
13 | char can_id = 0x01;
14 | 


--------------------------------------------------------------------------------
/wsg_50_driver/include/wsg_50/char.h:
--------------------------------------------------------------------------------
 1 | //======================================================================
 2 | /**
 3 |  *  @file
 4 |  *  char.h
 5 |  *
 6 |  *  @section char.h_general General file information
 7 |  *
 8 |  *  @brief
 9 |  *  
10 |  *
11 |  *  @author wolfer
12 |  *  @date	16.09.2011
13 |  *  
14 |  *  
15 |  *  @section char.h_copyright Copyright
16 |  *  
17 |  *  Copyright 2011 Weiss Robotics, D-71636 Ludwigsburg, Germany
18 |  *  
19 |  *  The distribution of this code and excerpts thereof, neither in 
20 |  *  source nor in any binary form, is prohibited, except you have our 
21 |  *  explicit and written permission to do so.
22 |  *
23 |  */
24 | //======================================================================
25 | 
26 | 
27 | #ifndef CHAR_H_
28 | #define CHAR_H_
29 | 
30 | //------------------------------------------------------------------------
31 | // Character constants
32 | //------------------------------------------------------------------------
33 | 
34 | /* Special character encodings */
35 | 
36 | #define ISO_NEWLINE     ( (unsigned char) 0x0a )	// '\ņ'
37 | #define ISO_TAB     	( (unsigned char) 0x09 )	// '\t'
38 | #define ISO_NULL		( (unsigned char) 0x00 )	// '\0'
39 | #define ISO_CARRIAGE	( (unsigned char) 0x0d )	// '\r' (Carriage return)
40 | #define ISO_ESC			( (unsigned char) 0x1b )	// '\033' (ESCAPE)
41 | #define ISO_SPACE    	( (unsigned char) 0x20 )	// ' '
42 | #define ISO_BANG     	( (unsigned char) 0x21 )	// '!'
43 | #define ISO_PERCENT  	( (unsigned char) 0x25 )	// '%'
44 | #define ISO_PERIOD   	( (unsigned char) 0x2e )	// '.'
45 | #define ISO_SLASH    	( (unsigned char) 0x2f )	// '/'
46 | #define ISO_BACKSLASH	( (unsigned char) 0x5c )	// '\'
47 | #define ISO_COLON    	( (unsigned char) 0x3a )	// ':'
48 | #define ISO_AMP			( (unsigned char) 0x26 )	// '&'
49 | #define ISO_HYPHEN		( (unsigned char) 0x2d )	// '-'
50 | #define ISO_QUOTE		( (unsigned char) 0x22 )	// '"'
51 | #define ISO_EQUALS		( (unsigned char) 0x3d )	// '='
52 | #define ISO_SEMICOLON	( (unsigned char) 0x3b )	// ';'
53 | #define ISO_QUESTION	( (unsigned char) 0x3f )	// '?'
54 | 
55 | 
56 | #endif /* CHAR_H_ */
57 | 


--------------------------------------------------------------------------------
/wsg_50_driver/include/wsg_50/checksum.h:
--------------------------------------------------------------------------------
 1 | //======================================================================
 2 | /**
 3 |  *  @file
 4 |  *  checksum.h
 5 |  *
 6 |  *  @section checksum.h_general General file information
 7 |  *
 8 |  *  @brief
 9 |  *  
10 |  *
11 |  *  @author wolfer
12 |  *  @date	19.07.2011
13 |  *  
14 |  *  
15 |  *  @section checksum.h_copyright Copyright
16 |  *  
17 |  *  Copyright 2011 Weiss Robotics, D-71636 Ludwigsburg, Germany
18 |  *  
19 |  *  The distribution of this code and excerpts thereof, neither in 
20 |  *  source nor in any binary form, is prohibited, except you have our 
21 |  *  explicit and written permission to do so.
22 |  *
23 |  */
24 | //======================================================================
25 | 
26 | 
27 | #ifndef CHECKSUM_H_
28 | #define CHECKSUM_H_
29 | 
30 | //------------------------------------------------------------------------
31 | // Includes
32 | //------------------------------------------------------------------------
33 | 
34 | 
35 | 
36 | //------------------------------------------------------------------------
37 | // Macros
38 | //------------------------------------------------------------------------
39 | 
40 | 
41 | 
42 | #ifdef __cplusplus
43 | extern "C" {
44 | #endif
45 | 
46 | //------------------------------------------------------------------------
47 | // Typedefs, enums, structs
48 | //------------------------------------------------------------------------
49 | 
50 | 
51 | //------------------------------------------------------------------------
52 | // Global variables
53 | //------------------------------------------------------------------------
54 | 
55 | 
56 | //------------------------------------------------------------------------
57 | // Function declaration
58 | //------------------------------------------------------------------------
59 | 
60 | unsigned short checksum_update_crc16( unsigned char *data, unsigned int size, unsigned short crc );
61 | unsigned short checksum_crc16( unsigned char *data, unsigned int size );
62 | 
63 | #ifdef __cplusplus
64 | }
65 | #endif
66 | 
67 | #endif /* CHECKSUM_H_ */
68 | 


--------------------------------------------------------------------------------
/wsg_50_driver/include/wsg_50/cmd.h:
--------------------------------------------------------------------------------
 1 | //======================================================================
 2 | /**
 3 |  *  @file
 4 |  *  cmd.h
 5 |  *
 6 |  *  @section cmd.h_general General file information
 7 |  *
 8 |  *  @brief
 9 |  *  Command abstraction layer (Header file)
10 |  *
11 |  *  @author wolfer
12 |  *  @date	20.07.2011
13 |  *  
14 |  *  
15 |  *  @section cmd.h_copyright Copyright
16 |  *  
17 |  *  Copyright 2011 Weiss Robotics, D-71636 Ludwigsburg, Germany
18 |  *  
19 |  *  The distribution of this code and excerpts thereof, neither in 
20 |  *  source nor in any binary form, is prohibited, except you have our 
21 |  *  explicit and written permission to do so.
22 |  *
23 |  */
24 | //======================================================================
25 | 
26 | 
27 | #ifndef CMD_H_
28 | #define CMD_H_
29 | 
30 | //------------------------------------------------------------------------
31 | // Includes
32 | //------------------------------------------------------------------------
33 | 
34 | #include "common.h"
35 | 
36 | 
37 | //------------------------------------------------------------------------
38 | // Macros
39 | //------------------------------------------------------------------------
40 | 
41 | 
42 | 
43 | #ifdef __cplusplus
44 | extern "C" {
45 | #endif
46 | 
47 | //------------------------------------------------------------------------
48 | // Typedefs, enums, structs
49 | //------------------------------------------------------------------------
50 | 
51 | 
52 | //------------------------------------------------------------------------
53 | // Global variables
54 | //------------------------------------------------------------------------
55 | 
56 | 
57 | //------------------------------------------------------------------------
58 | // Function declaration
59 | //------------------------------------------------------------------------
60 | 
61 | int cmd_connect_tcp( const char *addr, unsigned short port );
62 | int cmd_connect_udp( unsigned short local_port, const char *addr, unsigned short remote_port );
63 | int cmd_connect_serial( const char *device, unsigned int bitrate );
64 | 
65 | void cmd_disconnect( void );
66 | bool cmd_is_connected( void );
67 | status_t cmd_get_response_status( unsigned char *response );
68 | 
69 | int cmd_submit( unsigned char id, unsigned char *payload, unsigned int len,
70 | 			    bool pending, unsigned char **response, unsigned int *response_len );
71 | 
72 | 
73 | #ifdef __cplusplus
74 | }
75 | #endif
76 | 
77 | #endif /* CMD_H_ */
78 | 


--------------------------------------------------------------------------------
/wsg_50_driver/include/wsg_50/common.h:
--------------------------------------------------------------------------------
  1 | //======================================================================
  2 | /**
  3 |  *  @file
  4 |  *  common.h
  5 |  *
  6 |  *  @section common.h_general General file information
  7 |  *
  8 |  *  @brief
  9 |  *  
 10 |  *
 11 |  *  @author wolfer
 12 |  *  @date	07.07.2011
 13 |  *  
 14 |  *  
 15 |  *  @section common.h_copyright Copyright
 16 |  *  
 17 |  *  Copyright 2011 Weiss Robotics, D-71636 Ludwigsburg, Germany
 18 |  *  
 19 |  *  The distribution of this code and excerpts thereof, neither in 
 20 |  *  source nor in any binary form, is prohibited, except you have our 
 21 |  *  explicit and written permission to do so.
 22 |  *
 23 |  */
 24 | //======================================================================
 25 | 
 26 | 
 27 | #ifndef COMMON_H_
 28 | #define COMMON_H_
 29 | 
 30 | //------------------------------------------------------------------------
 31 | // Includes
 32 | //------------------------------------------------------------------------
 33 | 
 34 | 
 35 | //------------------------------------------------------------------------
 36 | // Macros
 37 | //------------------------------------------------------------------------
 38 | 
 39 | #define dbgPrint( fmt, ... )		fprintf( stderr, "%s()\t" fmt, __FUNCTION__ , ##__VA_ARGS__ )
 40 | #define dbgPrintPlain( fmt, ... )	fprintf( stderr, fmt, ##__VA_ARGS__ )
 41 | #define dbgPrintF( fmt, ... )		fprintf( stderr, "%s()\t" fmt, __FUNCTION__ , ##__VA_ARGS__ )
 42 | #define dbgPrintPlainF( fmt, ... )	fprintf( stderr, fmt, ##__VA_ARGS__ )
 43 | 
 44 | #define ASSERT( cond )				assert( cond )
 45 | 
 46 | //! Macro for detecting errors and exiting using the error code:
 47 | #define EXIT_ON_ERROR( error_code, msg ) \
 48 | 	do { \
 49 | 		if ( error_code != E_SUCCESS ) \
 50 | 		{ \
 51 | 			fprintf( stderr, "%s: ERROR %s: %s\n", __FUNCTION__, msg, error_to_str( error_code )); \
 52 | 			return( error_code ); \
 53 | 		} \
 54 | 	} while( 0 )
 55 | 
 56 | #define STATUS_DESCRIPTORS {	  \
 57 |     "E_SUCCESS", 	              \
 58 |     "E_NOT_AVAILABLE",            \
 59 |     "E_NO_SENSOR",                \
 60 |     "E_NOT_INITIALIZED",          \
 61 |     "E_ALREADY_RUNNING",          \
 62 |     "E_FEATURE_NOT_SUPPORTED",    \
 63 |     "E_INCONSISTENT_DATA",        \
 64 |     "E_TIMEOUT",                  \
 65 |     "E_READ_ERROR",               \
 66 |     "E_WRITE_ERROR",              \
 67 |     "E_INSUFFICIENT_RESOURCES",   \
 68 |     "E_CHECKSUM_ERROR",           \
 69 |     "E_NO_PARAM_EXPECTED",        \
 70 |     "E_NOT_ENOUGH_PARAMS",        \
 71 |     "E_CMD_UNKNOWN",              \
 72 |     "E_CMD_FORMAT_ERROR",         \
 73 |     "E_ACCESS_DENIED",            \
 74 |     "E_ALREADY_OPEN",             \
 75 |     "E_CMD_FAILED",               \
 76 |     "E_CMD_ABORTED",              \
 77 |     "E_INVALID_HANDLE",           \
 78 |     "E_NOT_FOUND",        		  \
 79 |     "E_NOT_OPEN",        		  \
 80 |     "E_IO_ERROR",                 \
 81 |     "E_INVALID_PARAMETER",        \
 82 |     "E_INDEX_OUT_OF_BOUNDS",      \
 83 |     "E_CMD_PENDING",              \
 84 |     "E_OVERRUN",				  \
 85 |     "E_RANGE_ERROR",			  \
 86 |     "E_AXIS_BLOCKED",			  \
 87 |     "E_FILE_EXISTS",			  \
 88 |     NULL	       				  \
 89 | }
 90 | 
 91 | 
 92 | #ifdef __cplusplus
 93 | extern "C" {
 94 | #endif
 95 | 
 96 | //------------------------------------------------------------------------
 97 | // Typedefs, enums, structs
 98 | //------------------------------------------------------------------------
 99 | 
100 | typedef enum {
101 |     E_SUCCESS = 0,              // No error
102 |     E_NOT_AVAILABLE,            // Device, service or data is not available
103 |     E_NO_SENSOR,                // No sensor connected
104 |     E_NOT_INITIALIZED,          // The device is not initialized
105 |     E_ALREADY_RUNNING,          // Service is already running
106 |     E_FEATURE_NOT_SUPPORTED,    // The asked feature is not supported
107 |     E_INCONSISTENT_DATA,        // One or more dependent parameters mismatch
108 |     E_TIMEOUT,                  // Timeout error
109 |     E_READ_ERROR,               // Error while reading from a device
110 |     E_WRITE_ERROR,              // Error while writing to a device
111 |     E_INSUFFICIENT_RESOURCES,   // No memory available
112 |     E_CHECKSUM_ERROR,           // Checksum error
113 |     E_NO_PARAM_EXPECTED,        // No parameters expected
114 |     E_NOT_ENOUGH_PARAMS,        // Not enough parameters
115 |     E_CMD_UNKNOWN,              // Unknown command
116 |     E_CMD_FORMAT_ERROR,         // Command format error
117 |     E_ACCESS_DENIED,            // Access denied
118 |     E_ALREADY_OPEN,             // The interface is already open
119 |     E_CMD_FAILED,               // Command failed
120 |     E_CMD_ABORTED,              // Command aborted
121 |     E_INVALID_HANDLE,           // invalid handle
122 |     E_NOT_FOUND,        	// device not found
123 |     E_NOT_OPEN,        		// device not open
124 |     E_IO_ERROR,                 // I/O error
125 |     E_INVALID_PARAMETER,        // invalid parameter
126 |     E_INDEX_OUT_OF_BOUNDS,      // index out of bounds
127 |     E_CMD_PENDING,              // Command was received correctly, but the execution needs more time. If the command was completely processed, another status message is returned indicating the command's result
128 |     E_OVERRUN,					// Data overrun
129 |     E_RANGE_ERROR,				// Range error
130 |     E_AXIS_BLOCKED,				// Axis is blocked
131 |     E_FILE_EXISTS				// File already exists
132 | } status_t;
133 | 
134 | 
135 | // IP address type
136 | typedef unsigned int ip_addr_t;
137 | 
138 | // Boolean type for ANSI C
139 | #ifndef __cplusplus
140 | 	typedef unsigned char bool;
141 | 	#define true		1
142 | 	#define false		0
143 | #endif
144 | 
145 | 
146 | // Devices
147 | typedef enum
148 | {
149 | 	UNKNOWN = 0,
150 | 	DSACON32,
151 | 	WSG50,
152 | 	WSG32,
153 | 	KMS40
154 | } device_t;
155 | 
156 | 
157 | //------------------------------------------------------------------------
158 | // Function declaration
159 | //------------------------------------------------------------------------
160 | 
161 | ip_addr_t str_to_ipaddr( const char *str );
162 | const char * status_to_str( status_t status );
163 | void quit( const char *reason );
164 | const char * getStateValues( unsigned char * b);
165 | 
166 | #ifdef __cplusplus
167 | }
168 | #endif
169 | 
170 | #endif /* COMMON_H_ */
171 | 


--------------------------------------------------------------------------------
/wsg_50_driver/include/wsg_50/functions.h:
--------------------------------------------------------------------------------
  1 | //======================================================================
  2 | /**
  3 |  *  @file
  4 |  *  functions.h
  5 |  *
  6 |  *  @section testing.h_general General file information
  7 |  *
  8 |  *  @brief
  9 |  *  
 10 |  *
 11 |  *  @author wolfer
 12 |  *  @date	30.04.2012
 13 |  *  
 14 |  *  
 15 |  *  @section testing.h_copyright Copyright
 16 |  *  
 17 |  *  Copyright 2012 Weiss Robotics, D-71636 Ludwigsburg, Germany
 18 |  *  
 19 |  *  The distribution of this code and excerpts thereof, neither in 
 20 |  *  source nor in any binary form, is prohibited, except you have our 
 21 |  *  explicit and written permission to do so.
 22 |  *
 23 |  */
 24 | //======================================================================
 25 | 
 26 | 
 27 | #ifndef FUNCTIONS_H_
 28 | #define FUNCTIONS_H_
 29 | 
 30 |  #include <string>
 31 | 
 32 | //------------------------------------------------------------------------
 33 | // Includes
 34 | //------------------------------------------------------------------------
 35 | 
 36 | 
 37 | //------------------------------------------------------------------------
 38 | // Macros
 39 | //------------------------------------------------------------------------
 40 | 
 41 | 
 42 | 
 43 | /*#ifdef __cplusplus
 44 | extern "C" {
 45 | #endif */
 46 | 
 47 | //------------------------------------------------------------------------
 48 | // Typedefs, enums, structs
 49 | //------------------------------------------------------------------------
 50 | typedef struct {
 51 | 	unsigned int state;
 52 | 	bool ismoving;
 53 | 	float position, speed;
 54 | 	float f_motor, f_finger0, f_finger1;
 55 | 	std::string state_text;
 56 | } gripper_response;
 57 | 
 58 | //------------------------------------------------------------------------
 59 | // Global variables
 60 | //------------------------------------------------------------------------
 61 | 
 62 | 
 63 | 
 64 | //------------------------------------------------------------------------
 65 | // Function declaration
 66 | //------------------------------------------------------------------------
 67 | 
 68 | float convert(unsigned char *b);
 69 | int homing( void );
 70 | int move(float width, float speed, bool stop_on_block, bool ignore_response = false);
 71 | int stop( bool ignore_response = false );
 72 | int grasp( float objWidth, float speed );
 73 | int release( float width, float speed );
 74 | int ack_fault( void );
 75 | 
 76 | int setAcceleration( float acc );
 77 | int setGraspingForceLimit( float force );
 78 | int doTare( void );
 79 | 
 80 | const char * systemState( void );
 81 | int graspingState( void );
 82 | float getOpening(int auto_update = 0);
 83 | float getForce(int auto_update = 0);
 84 | float getSpeed(int auto_update = 0);
 85 | int getAcceleration( void );
 86 | int getGraspingForceLimit( void );
 87 | 
 88 | int script_measure_move (unsigned char cmd_type, float cmd_width, float cmd_speed, gripper_response & info);
 89 | 
 90 | //void getStateValues(); //(unsigned char *);
 91 | 
 92 | //void test( void );
 93 | 
 94 | 
 95 | /*#ifdef __cplusplus
 96 | }
 97 | #endif */
 98 | 
 99 | #endif /* FUNCTIONS_H_ */
100 | 


--------------------------------------------------------------------------------
/wsg_50_driver/include/wsg_50/functions_can.h:
--------------------------------------------------------------------------------
 1 | //======================================================================
 2 | /**
 3 |  *  @file
 4 |  *  functions_can.h
 5 |  *
 6 |  *  @section functions_can.h_general General file information
 7 |  *
 8 |  *  @brief
 9 |  *  
10 |  *
11 |  *  @author Marc Benetó
12 |  *  @date   14.09.2012
13 |  *  
14 |  *  
15 |  *  @section functions_can.h_copyright Copyright
16 |  *  
17 |  *  Copyright 2012 Robotnik Automation, SLL
18 |  *  
19 |  *  The distribution of this code and excerpts thereof, neither in 
20 |  *  source nor in any binary form, is prohibited, except you have our 
21 |  *  explicit and written permission to do so.
22 |  *
23 |  */
24 | //======================================================================
25 | 
26 | 
27 | #ifndef FUNCTIONS_H_
28 | #define FUNCTIONS_H_
29 | 
30 | //------------------------------------------------------------------------
31 | // Includes
32 | //------------------------------------------------------------------------
33 | 
34 | 
35 | //------------------------------------------------------------------------
36 | // Macros
37 | //------------------------------------------------------------------------
38 | 
39 | #ifdef __cplusplus
40 | extern "C" {
41 | #endif
42 | 
43 | //------------------------------------------------------------------------
44 | // Typedefs, enums, structs
45 | //------------------------------------------------------------------------
46 | 
47 | //------------------------------------------------------------------------
48 | // Global variables
49 | //------------------------------------------------------------------------
50 | 
51 | 
52 | //------------------------------------------------------------------------
53 | // Function declaration
54 | //------------------------------------------------------------------------
55 | 
56 | // CAN bus management functions
57 | bool CAN_connect( const char *dev );
58 | void CAN_disconnect( void );
59 | 
60 | // Goto/grasp commands
61 | void homing( void );
62 | int move( float width, float speed );
63 | int grasp( float objWidth, float speed );
64 | int release( float width, float speed );
65 | //int stop( void );
66 | //int ack_fault( void );
67 | 
68 | // Set functions
69 | void setAcceleration( float acc );
70 | void setGraspingForceLimit( float force );
71 | 
72 | // Get functions
73 | float getOpening( void );
74 | float getGraspingForceLimit( void );
75 | float getAcceleration( void );
76 | //const char * systemState( void );
77 | //int graspingState( void );
78 | 
79 | 
80 | #ifdef __cplusplus
81 | }
82 | #endif
83 | 
84 | #endif /* FUNCTIONS_H_ */
85 | 


--------------------------------------------------------------------------------
/wsg_50_driver/include/wsg_50/interface.h:
--------------------------------------------------------------------------------
 1 | //======================================================================
 2 | /**
 3 |  *  @file
 4 |  *  interface.h
 5 |  *
 6 |  *  @section interface.h_general General file information
 7 |  *
 8 |  *  @brief
 9 |  *  
10 |  *
11 |  *  @author wolfer
12 |  *  @date	07.07.2011
13 |  *  
14 |  *  
15 |  *  @section interface.h_copyright Copyright
16 |  *  
17 |  *  Copyright 2011 Weiss Robotics, D-71636 Ludwigsburg, Germany
18 |  *  
19 |  *  The distribution of this code and excerpts thereof, neither in 
20 |  *  source nor in any binary form, is prohibited, except you have our 
21 |  *  explicit and written permission to do so.
22 |  *
23 |  */
24 | //======================================================================
25 | 
26 | 
27 | #ifndef INTERFACE_H_
28 | #define INTERFACE_H_
29 | 
30 | //------------------------------------------------------------------------
31 | // Includes
32 | //------------------------------------------------------------------------
33 | 
34 | 
35 | 
36 | #ifdef __cplusplus
37 | extern "C" {
38 | #endif
39 | 
40 | //------------------------------------------------------------------------
41 | // Macros
42 | //------------------------------------------------------------------------
43 | 
44 | 
45 | //------------------------------------------------------------------------
46 | // Typedefs, enums, structs
47 | //------------------------------------------------------------------------
48 | 
49 | typedef struct
50 | {
51 | 	const char *name;
52 | 	int ( *open ) ( const void *params );
53 | 	void ( *close ) ( void );
54 | 	int ( *read ) ( unsigned char *, unsigned int );
55 | 	int ( *write ) ( unsigned char *, unsigned int );
56 | } interface_t;
57 | 
58 | 
59 | //------------------------------------------------------------------------
60 | // Function declaration
61 | //------------------------------------------------------------------------
62 | 
63 | const interface_t * interface_get( const char *name );
64 | 
65 | 
66 | #ifdef __cplusplus
67 | }
68 | #endif
69 | 
70 | #endif /* INTERFACE_H_ */
71 | 


--------------------------------------------------------------------------------
/wsg_50_driver/include/wsg_50/msg.h:
--------------------------------------------------------------------------------
 1 | //======================================================================
 2 | /**
 3 |  *  @file
 4 |  *  msg.h
 5 |  *
 6 |  *  @section msg.h_general General file information
 7 |  *
 8 |  *  @brief
 9 |  *  Raw send and receive functions for command messages (Header file)
10 |  *
11 |  *  @author wolfer
12 |  *  @date	19.07.2011
13 |  *  
14 |  *  
15 |  *  @section msg.h_copyright Copyright
16 |  *  
17 |  *  Copyright 2011 Weiss Robotics, D-71636 Ludwigsburg, Germany
18 |  *  
19 |  *  The distribution of this code and excerpts thereof, neither in 
20 |  *  source nor in any binary form, is prohibited, except you have our 
21 |  *  explicit and written permission to do so.
22 |  *
23 |  */
24 | //======================================================================
25 | 
26 | 
27 | #ifndef MSG_H_
28 | #define MSG_H_
29 | 
30 | //------------------------------------------------------------------------
31 | // Includes
32 | //------------------------------------------------------------------------
33 | 
34 | #include "common.h"
35 | #include "interface.h"
36 | 
37 | 
38 | //------------------------------------------------------------------------
39 | // Macros
40 | //------------------------------------------------------------------------
41 | 
42 | #define MSG_PREAMBLE_BYTE		0xaa
43 | #define MSG_PREAMBLE_LEN		3
44 | 
45 | // Combine bytes to different types
46 | #define make_short( lowbyte, highbyte )				( (unsigned short)lowbyte | ( (unsigned short)highbyte << 8 ) )
47 | #define make_signed_short( lowbyte, highbyte )		( (signed short) ( (unsigned short) lowbyte | ( (unsigned short) highbyte << 8 ) ) )
48 | #define make_int( lowbyte, mid1, mid2, highbyte )	( (unsigned int) lowbyte | ( (unsigned int) mid1 << 8 ) | ( (unsigned int) mid2 << 16 ) | ( (unsigned int) highbyte << 24 ) )
49 | #define make_float( result, byteptr )				memcpy( &result, byteptr, sizeof( float ) )
50 | 
51 | // Byte access
52 | #define hi( x )    	(unsigned char) ( ((x) >> 8) & 0xff )	// Returns the upper byte of the passed short
53 | #define lo( x )    	(unsigned char) ( (x) & 0xff )       	// Returns the lower byte of the passed short
54 | 
55 | 
56 | #ifdef __cplusplus
57 | extern "C" {
58 | #endif
59 | 
60 | //------------------------------------------------------------------------
61 | // Typedefs, enums, structs
62 | //------------------------------------------------------------------------
63 | 
64 | typedef struct
65 | {
66 | 	unsigned char id;
67 | 	unsigned int len;
68 | 	unsigned char *data;
69 | } msg_t;
70 | 
71 | 
72 | //------------------------------------------------------------------------
73 | // Global variables
74 | //------------------------------------------------------------------------
75 | 
76 | 
77 | //------------------------------------------------------------------------
78 | // Function declaration
79 | //------------------------------------------------------------------------
80 | 
81 | int msg_open( const interface_t *iface, const void *params );
82 | void msg_close( void );
83 | int msg_change_interface( const interface_t *iface );
84 | int msg_send( msg_t *msg );
85 | int msg_receive( msg_t *msg );
86 | void msg_free( msg_t *msg );
87 | 
88 | #ifdef __cplusplus
89 | }
90 | #endif
91 | 
92 | #endif /* MSG_H_ */
93 | 


--------------------------------------------------------------------------------
/wsg_50_driver/include/wsg_50/serial.h:
--------------------------------------------------------------------------------
 1 | //======================================================================
 2 | /**
 3 |  *  @file
 4 |  *  serial.h
 5 |  *
 6 |  *  @section serial.h_general General file information
 7 |  *
 8 |  *  @brief
 9 |  *  
10 |  *
11 |  *  @author wolfer
12 |  *  @date	08.07.2011
13 |  *  
14 |  *  
15 |  *  @section serial.h_copyright Copyright
16 |  *  
17 |  *  Copyright 2011 Weiss Robotics, D-71636 Ludwigsburg, Germany
18 |  *  
19 |  *  The distribution of this code and excerpts thereof, neither in 
20 |  *  source nor in any binary form, is prohibited, except you have our 
21 |  *  explicit and written permission to do so.
22 |  *
23 |  */
24 | //======================================================================
25 | 
26 | 
27 | #ifndef SERIAL_H_
28 | #define SERIAL_H_
29 | 
30 | //------------------------------------------------------------------------
31 | // Includes
32 | //------------------------------------------------------------------------
33 | 
34 | 
35 | 
36 | #ifdef __cplusplus
37 | extern "C" {
38 | #endif
39 | 
40 | //------------------------------------------------------------------------
41 | // Macros
42 | //------------------------------------------------------------------------
43 | 
44 | 
45 | //------------------------------------------------------------------------
46 | // Typedefs, enums, structs
47 | //------------------------------------------------------------------------
48 | 
49 | typedef struct
50 | {
51 | 	const char *device;
52 | 	unsigned int bitrate;
53 | } ser_params_t;
54 | 
55 | 
56 | typedef struct
57 | {
58 | 	int fd;
59 | } ser_conn_t;
60 | 
61 | 
62 | //------------------------------------------------------------------------
63 | // Function declaration
64 | //------------------------------------------------------------------------
65 | 
66 | int serial_open( const void *params );
67 | void serial_close( void );
68 | int serial_read( unsigned char *buf, unsigned int len );
69 | int serial_write( unsigned char *buf, unsigned int len );
70 | 
71 | 
72 | #ifdef __cplusplus
73 | }
74 | #endif
75 | 
76 | #endif /* SERIAL_H_ */
77 | 


--------------------------------------------------------------------------------
/wsg_50_driver/include/wsg_50/tcp.h:
--------------------------------------------------------------------------------
 1 | //======================================================================
 2 | /**
 3 |  *  @file
 4 |  *  tcp.h
 5 |  *
 6 |  *  @section tcp.h_general General file information
 7 |  *
 8 |  *  @brief
 9 |  *  
10 |  *
11 |  *  @author wolfer
12 |  *  @date	08.07.2011
13 |  *  
14 |  *  
15 |  *  @section tcp.h_copyright Copyright
16 |  *  
17 |  *  Copyright 2011 Weiss Robotics, D-71636 Ludwigsburg, Germany
18 |  *  
19 |  *  The distribution of this code and excerpts thereof, neither in 
20 |  *  source nor in any binary form, is prohibited, except you have our 
21 |  *  explicit and written permission to do so.
22 |  *
23 |  */
24 | //======================================================================
25 | 
26 | 
27 | #ifndef TCP_H_
28 | #define TCP_H_
29 | 
30 | //------------------------------------------------------------------------
31 | // Includes
32 | //------------------------------------------------------------------------
33 | 
34 | #ifdef WIN32
35 | 	// Note: Compiler has to link against -lwsock32 or -lws2_32 on MinGW
36 | 	// @todo Have to adjust some code to make tcp work on MinGW
37 | 	#include "winsock.h"
38 | 	#include "winsock2.h"
39 | #else
40 | 	#include <unistd.h>
41 | 	#include <sys/types.h>
42 | 	#include <sys/socket.h>
43 | 	#include <sys/ioctl.h>
44 | 	#include <sys/select.h>
45 | 	#include <arpa/inet.h>
46 | 	#include <netinet/in.h>
47 | #endif
48 | 
49 | #include "common.h"
50 | 
51 | 
52 | #ifdef __cplusplus
53 | extern "C" {
54 | #endif
55 | 
56 | //------------------------------------------------------------------------
57 | // Macros
58 | //------------------------------------------------------------------------
59 | 
60 | 
61 | //------------------------------------------------------------------------
62 | // Typedefs, enums, structs
63 | //------------------------------------------------------------------------
64 | 
65 | typedef struct
66 | {
67 | 	ip_addr_t addr;
68 | 	unsigned short port;
69 | } tcp_params_t;
70 | 
71 | 
72 | typedef struct
73 | {
74 | 	int sock;
75 | 	struct sockaddr_in si_server;
76 | 	ip_addr_t server;
77 | } tcp_conn_t;
78 | 
79 | 
80 | //------------------------------------------------------------------------
81 | // Function declaration
82 | //------------------------------------------------------------------------
83 | 
84 | int tcp_open( const void *params );
85 | void tcp_close( void );
86 | int tcp_read( unsigned char *buf, unsigned int len );
87 | int tcp_write( unsigned char *buf, unsigned int len );
88 | 
89 | 
90 | #ifdef __cplusplus
91 | }
92 | #endif
93 | 
94 | #endif /* TCP_H_ */
95 | 


--------------------------------------------------------------------------------
/wsg_50_driver/include/wsg_50/udp.h:
--------------------------------------------------------------------------------
  1 | //======================================================================
  2 | /**
  3 |  *  @file
  4 |  *  udp.h
  5 |  *
  6 |  *  @section udp.h_general General file information
  7 |  *
  8 |  *  @brief
  9 |  *  
 10 |  *
 11 |  *  @author wolfer
 12 |  *  @date	07.07.2011
 13 |  *  
 14 |  *  
 15 |  *  @section udp.h_copyright Copyright
 16 |  *  
 17 |  *  Copyright 2011 Weiss Robotics, D-71636 Ludwigsburg, Germany
 18 |  *  
 19 |  *  The distribution of this code and excerpts thereof, neither in 
 20 |  *  source nor in any binary form, is prohibited, except you have our 
 21 |  *  explicit and written permission to do so.
 22 |  *
 23 |  */
 24 | //======================================================================
 25 | 
 26 | 
 27 | #ifndef UDP_H_
 28 | #define UDP_H_
 29 | 
 30 | //------------------------------------------------------------------------
 31 | // Includes
 32 | //------------------------------------------------------------------------
 33 | 
 34 | #ifdef WIN32
 35 | // Note: Compiler has to link against -lwsock32 or -lws2_32 on MinGW
 36 | // @todo Have to adjust some code to make udp work on MinGW
 37 | #include "winsock.h"
 38 | #include "winsock2.h"
 39 | #else
 40 | #include <unistd.h>
 41 | #include <sys/types.h>
 42 | #include <sys/socket.h>
 43 | #include <sys/ioctl.h>
 44 | #include <sys/select.h>
 45 | #include <arpa/inet.h>
 46 | #include <netinet/in.h>
 47 | #endif
 48 | 
 49 | #include "common.h"
 50 | 
 51 | 
 52 | #ifdef __cplusplus
 53 | extern "C" {
 54 | #endif
 55 | 
 56 | //------------------------------------------------------------------------
 57 | // Macros
 58 | //------------------------------------------------------------------------
 59 | 
 60 | #define UDP_RCV_BUFSIZE		1024		// Size of UDP receive buffer. This is the maximum size a command message may have, including preamble etc.
 61 | 										// Minimum is 8 (3 bytes preamble, 1 byte command id, 2 bytes size, 2 bytes checksum)
 62 | 
 63 | 
 64 | //------------------------------------------------------------------------
 65 | // Typedefs, enums, structs
 66 | //------------------------------------------------------------------------
 67 | 
 68 | typedef struct
 69 | {
 70 | 	unsigned short local_port;
 71 | 	ip_addr_t addr;
 72 | 	unsigned short remote_port;
 73 | } udp_params_t;
 74 | 
 75 | 
 76 | typedef struct
 77 | {
 78 | 	int sock;
 79 | 	unsigned char rcv_buf[UDP_RCV_BUFSIZE];
 80 | 	unsigned int rcv_bufptr;
 81 | 	unsigned int rcv_bufsize;
 82 | 	struct sockaddr_in si_listen;
 83 | 	struct sockaddr_in si_server;
 84 | 	struct sockaddr_in si_incoming;
 85 | 	ip_addr_t server;
 86 | } udp_conn_t;
 87 | 
 88 | 
 89 | //------------------------------------------------------------------------
 90 | // Function declaration
 91 | //------------------------------------------------------------------------
 92 | 
 93 | int udp_open( const void *params );
 94 | void udp_close( void );
 95 | int udp_read( unsigned char *buf, unsigned int len );
 96 | int udp_write( unsigned char *buf, unsigned int len );
 97 | 
 98 | 
 99 | #ifdef __cplusplus
100 | }
101 | #endif
102 | 
103 | #endif /* UDP_H_ */
104 | 


--------------------------------------------------------------------------------
/wsg_50_driver/launch/wsg_50_can.launch:
--------------------------------------------------------------------------------
 1 | <?xml version="1.0"?>
 2 | <launch>
 3 | 
 4 |   <node  name="wsg_50_can"  pkg="wsg_50" type="wsg_50_can">
 5 | 	<param name="device" type="string" value="/dev/pcan32"/>
 6 |   </node>
 7 | 
 8 | </launch>
 9 | 
10 | 


--------------------------------------------------------------------------------
/wsg_50_driver/launch/wsg_50_tcp.launch:
--------------------------------------------------------------------------------
 1 | <?xml version="1.0"?>
 2 | <launch>
 3 | 
 4 |   <node name="wsg_50_driver"  pkg="wsg_50_driver" type="wsg_50_ip">
 5 | 	<param name="ip" type="string" value="192.168.1.20"/>
 6 | 	<param name="port" type="int" value="1000"/>
 7 |   </node>
 8 | 
 9 | </launch>
10 | 
11 | 


--------------------------------------------------------------------------------
/wsg_50_driver/launch/wsg_50_tcp_script.launch:
--------------------------------------------------------------------------------
 1 | <launch>
 2 |   <arg name="com_mode" default="script" /><!-- or auto_update, polling -->
 3 |   <arg name="remap" default="false" /><!-- Remap joint_states topic. e.g. for joint_state_publisher -->
 4 |   <arg name="force" default="10" />
 5 | 
 6 |   <remap from="/joint_states" to="/joint_states_gripper" if="$(arg remap)"/>
 7 | 
 8 |   <node  name="wsg_50_driver"  pkg="wsg_50_driver" type="wsg_50_ip" output="screen">
 9 | 	<param name="ip" type="string" value="192.168.20.2"/>
10 | 	<param name="port" type="int" value="1000"/>
11 |     <param name="protocol" type="string" value="tcp"/>
12 |     <param name="com_mode" type="string" value="$(arg com_mode)"/>
13 |     <param name="rate" type="double" value="50"/> <!-- WSG50 HW revision 2: up to 30 Hz with script; 140Hz with auto_update -->
14 |     <param name="grasping_force" type="double" value="$(arg force)"/>
15 |   </node>
16 | 
17 | </launch>
18 | 
19 | 


--------------------------------------------------------------------------------
/wsg_50_driver/launch/wsg_50_udp_script.launch:
--------------------------------------------------------------------------------
 1 | <launch>
 2 |   <arg name="com_mode" default="script" /><!-- or auto_update, polling -->
 3 |   <arg name="remap" default="false" /><!-- Remap joint_states topic. e.g. for joint_state_publisher -->
 4 |   <arg name="force" default="10" />
 5 | 
 6 |   <remap from="/joint_states" to="/joint_states_gripper" if="$(arg remap)"/>
 7 |   
 8 |   <node  name="wsg_50_driver"  pkg="wsg_50_driver" type="wsg_50_ip" output="screen" launch-prefix="gdb --args">
 9 | 	  <param name="ip" type="string" value="192.168.20.2"/>
10 | 	  <param name="port" type="int" value="1500"/>
11 | 	  <param name="local_port" type="int" value="5501"/>
12 |     <param name="protocol" type="string" value="udp"/>
13 |     <param name="com_mode" type="string" value="$(arg com_mode)"/>
14 |     <param name="rate" type="double" value="50"/>
15 |     <param name="grasping_force" type="double" value="$(arg force)"/>
16 |   </node>
17 | 
18 | </launch>
19 | 
20 | 


--------------------------------------------------------------------------------
/wsg_50_driver/package.xml:
--------------------------------------------------------------------------------
 1 | <?xml version="1.0"?>
 2 | <package>
 3 |   <name>wsg_50_driver</name>
 4 |   <version>0.0.0</version>
 5 |   <description>The wsg_50_driver package</description>
 6 | 
 7 |   <!-- One maintainer tag required, multiple allowed, one person per tag --> 
 8 |   <!-- Example:  -->
 9 |   <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
10 |   <maintainer email="nalt@todo.todo">nalt</maintainer>
11 | 
12 | 
13 |   <!-- One license tag required, multiple allowed, one license per tag -->
14 |   <!-- Commonly used license strings: -->
15 |   <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
16 |   <license>BSD</license>
17 | 
18 | 
19 |   <!-- Url tags are optional, but mutiple are allowed, one per tag -->
20 |   <!-- Optional attribute type can be: website, bugtracker, or repository -->
21 |   <!-- Example: -->
22 |   <!-- <url type="website">http://wiki.ros.org/wsg_50</url> -->
23 | 
24 | 
25 |   <!-- Author tags are optional, mutiple are allowed, one per tag -->
26 |   <!-- Authors do not have to be maintianers, but could be -->
27 |   <!-- Example: -->
28 |   <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
29 |   <author >Nicolas Alt</author>
30 |   <author >Robotnik</author>
31 |   <author >Weiss Robotics</author>
32 | 
33 | 
34 |   <!-- The *_depend tags are used to specify dependencies -->
35 |   <!-- Dependencies can be catkin packages or system dependencies -->
36 |   <!-- Examples: -->
37 |   <!-- Use build_depend for packages you need at compile time: -->
38 |   <!--   <build_depend>message_generation</build_depend> -->
39 |   <!-- Use buildtool_depend for build tool packages: -->
40 |   <!--   <buildtool_depend>catkin</buildtool_depend> -->
41 |   <!-- Use run_depend for packages you need at runtime: -->
42 |   <!--   <run_depend>message_runtime</run_depend> -->
43 |   <!-- Use test_depend for packages you need only for testing: -->
44 |   <!--   <test_depend>gtest</test_depend> -->
45 |   <buildtool_depend>catkin</buildtool_depend>
46 |   <build_depend>roscpp</build_depend>
47 |   <build_depend>std_msgs</build_depend>
48 |   <build_depend>std_srvs</build_depend>
49 |   <build_depend>wsg_50_common</build_depend>
50 |   <run_depend>roscpp</run_depend>
51 |   <run_depend>std_msgs</run_depend>
52 |   <run_depend>std_srvs</run_depend>
53 |   <run_depend>wsg_50_common</run_depend>
54 | 
55 | 
56 |   <!-- The export tag contains other, unspecified, tags -->
57 |   <export>
58 |     <!-- You can specify that this package is a metapackage here: -->
59 |     <!-- <metapackage/> -->
60 | 
61 |     <!-- Other tools can request additional information be placed here -->
62 | 
63 |   </export>
64 | </package>


--------------------------------------------------------------------------------
/wsg_50_driver/src/checksum.cpp:
--------------------------------------------------------------------------------
  1 | //======================================================================
  2 | /**
  3 |  *  @file
  4 |  *  checksum.c
  5 |  *
  6 |  *  @section checksum.c_general General file information
  7 |  *
  8 |  *  @brief
  9 |  *  Checksum functions. Taken from wrOS.
 10 |  *  
 11 |  *
 12 |  *  @author wolfer
 13 |  *  @date	19.07.2011
 14 |  *  
 15 |  *  
 16 |  *  @section checksum.c_copyright Copyright
 17 |  *  
 18 |  *  Copyright 2011 Weiss Robotics, D-71636 Ludwigsburg, Germany
 19 |  *  
 20 |  *  Redistribution and use in source and binary forms, with or without
 21 |  *  modification, are permitted provided that the following conditions are met:
 22 |  *
 23 |  *     * Redistributions of source code must retain the above copyright
 24 |  *       notice, this list of conditions and the following disclaimer.
 25 |  *     * Redistributions in binary form must reproduce the above copyright
 26 |  *       notice, this list of conditions and the following disclaimer in the
 27 |  *       documentation and/or other materials provided with the distribution.
 28 |  *     * Neither the name of the and Weiss Robotics GmbH nor the names of its 
 29 |  *       contributors may be used to endorse or promote products derived from
 30 |  *	 this software without specific prior written permission.
 31 |  *
 32 |  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 33 |  *  AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 34 |  *  IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 35 |  *  ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
 36 |  *  LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 37 |  *  CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 38 |  *  SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 39 |  *  INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 40 |  *  CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 41 |  *  ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 42 |  *  POSSIBILITY OF SUCH DAMAGE.
 43 |  *
 44 |  */
 45 | //======================================================================
 46 | 
 47 | 
 48 | 
 49 | //------------------------------------------------------------------------
 50 | // Includes
 51 | //------------------------------------------------------------------------
 52 | 
 53 | #include "wsg_50/checksum.h"
 54 | 
 55 | 
 56 | //------------------------------------------------------------------------
 57 | // Local macros
 58 | //------------------------------------------------------------------------
 59 | 
 60 | 
 61 | //------------------------------------------------------------------------
 62 | // Typedefs, enums, structs
 63 | //------------------------------------------------------------------------
 64 | 
 65 | 
 66 | //------------------------------------------------------------------------
 67 | // Global variables
 68 | //------------------------------------------------------------------------
 69 | 
 70 | /*
 71 |  * CRC16 lookup table
 72 |  *
 73 |  * CCITT-16 compatible crc table using polynomial 0x1021,
 74 |  * corresponding to x^16 + x^12 + x^5 + 1
 75 |  */
 76 | 
 77 | static const unsigned short CRC_TABLE_CCITT16[256] = {
 78 |     0x0000, 0x1021, 0x2042, 0x3063, 0x4084, 0x50a5, 0x60c6, 0x70e7,
 79 |     0x8108, 0x9129, 0xa14a, 0xb16b, 0xc18c, 0xd1ad, 0xe1ce, 0xf1ef,
 80 |     0x1231, 0x0210, 0x3273, 0x2252, 0x52b5, 0x4294, 0x72f7, 0x62d6,
 81 |     0x9339, 0x8318, 0xb37b, 0xa35a, 0xd3bd, 0xc39c, 0xf3ff, 0xe3de,
 82 |     0x2462, 0x3443, 0x0420, 0x1401, 0x64e6, 0x74c7, 0x44a4, 0x5485,
 83 |     0xa56a, 0xb54b, 0x8528, 0x9509, 0xe5ee, 0xf5cf, 0xc5ac, 0xd58d,
 84 |     0x3653, 0x2672, 0x1611, 0x0630, 0x76d7, 0x66f6, 0x5695, 0x46b4,
 85 |     0xb75b, 0xa77a, 0x9719, 0x8738, 0xf7df, 0xe7fe, 0xd79d, 0xc7bc,
 86 |     0x48c4, 0x58e5, 0x6886, 0x78a7, 0x0840, 0x1861, 0x2802, 0x3823,
 87 |     0xc9cc, 0xd9ed, 0xe98e, 0xf9af, 0x8948, 0x9969, 0xa90a, 0xb92b,
 88 |     0x5af5, 0x4ad4, 0x7ab7, 0x6a96, 0x1a71, 0x0a50, 0x3a33, 0x2a12,
 89 |     0xdbfd, 0xcbdc, 0xfbbf, 0xeb9e, 0x9b79, 0x8b58, 0xbb3b, 0xab1a,
 90 |     0x6ca6, 0x7c87, 0x4ce4, 0x5cc5, 0x2c22, 0x3c03, 0x0c60, 0x1c41,
 91 |     0xedae, 0xfd8f, 0xcdec, 0xddcd, 0xad2a, 0xbd0b, 0x8d68, 0x9d49,
 92 |     0x7e97, 0x6eb6, 0x5ed5, 0x4ef4, 0x3e13, 0x2e32, 0x1e51, 0x0e70,
 93 |     0xff9f, 0xefbe, 0xdfdd, 0xcffc, 0xbf1b, 0xaf3a, 0x9f59, 0x8f78,
 94 |     0x9188, 0x81a9, 0xb1ca, 0xa1eb, 0xd10c, 0xc12d, 0xf14e, 0xe16f,
 95 |     0x1080, 0x00a1, 0x30c2, 0x20e3, 0x5004, 0x4025, 0x7046, 0x6067,
 96 |     0x83b9, 0x9398, 0xa3fb, 0xb3da, 0xc33d, 0xd31c, 0xe37f, 0xf35e,
 97 |     0x02b1, 0x1290, 0x22f3, 0x32d2, 0x4235, 0x5214, 0x6277, 0x7256,
 98 |     0xb5ea, 0xa5cb, 0x95a8, 0x8589, 0xf56e, 0xe54f, 0xd52c, 0xc50d,
 99 |     0x34e2, 0x24c3, 0x14a0, 0x0481, 0x7466, 0x6447, 0x5424, 0x4405,
100 |     0xa7db, 0xb7fa, 0x8799, 0x97b8, 0xe75f, 0xf77e, 0xc71d, 0xd73c,
101 |     0x26d3, 0x36f2, 0x0691, 0x16b0, 0x6657, 0x7676, 0x4615, 0x5634,
102 |     0xd94c, 0xc96d, 0xf90e, 0xe92f, 0x99c8, 0x89e9, 0xb98a, 0xa9ab,
103 |     0x5844, 0x4865, 0x7806, 0x6827, 0x18c0, 0x08e1, 0x3882, 0x28a3,
104 |     0xcb7d, 0xdb5c, 0xeb3f, 0xfb1e, 0x8bf9, 0x9bd8, 0xabbb, 0xbb9a,
105 |     0x4a75, 0x5a54, 0x6a37, 0x7a16, 0x0af1, 0x1ad0, 0x2ab3, 0x3a92,
106 |     0xfd2e, 0xed0f, 0xdd6c, 0xcd4d, 0xbdaa, 0xad8b, 0x9de8, 0x8dc9,
107 |     0x7c26, 0x6c07, 0x5c64, 0x4c45, 0x3ca2, 0x2c83, 0x1ce0, 0x0cc1,
108 |     0xef1f, 0xff3e, 0xcf5d, 0xdf7c, 0xaf9b, 0xbfba, 0x8fd9, 0x9ff8,
109 |     0x6e17, 0x7e36, 0x4e55, 0x5e74, 0x2e93, 0x3eb2, 0x0ed1, 0x1ef0
110 | };
111 | 
112 | 
113 | //------------------------------------------------------------------------
114 | // Unit testing
115 | //------------------------------------------------------------------------
116 | 
117 | 
118 | //------------------------------------------------------------------------
119 | // Local function prototypes
120 | //------------------------------------------------------------------------
121 | 
122 | 
123 | //------------------------------------------------------------------------
124 | // Function implementation
125 | //------------------------------------------------------------------------
126 | 
127 | /**
128 |  * Calculates the CRC16 checksum of an array by using a table.
129 |  * The crc16 polynomial is 0x1021 ( x^16 + x^12 + x^5 + 1 ).
130 |  *
131 |  * Note: The checksum generated by this function is NOT according
132 |  * to CCITT standard!
133 |  *
134 |  * @param *data       Points to the byte array from which checksum should
135 |  *                    be calculated
136 |  * @param size        Size of the byte array
137 |  * @param crc         Value calculated over another array and start value
138 |  *                    of the crc16 calculation
139 |  *
140 |  * @return CRC16 checksum
141 |  */
142 | 
143 | unsigned short checksum_update_crc16( unsigned char *data, unsigned int size, unsigned short crc )
144 | {
145 |     unsigned long c;
146 | 
147 | 	/* process each byte prior to checksum field */
148 | 	for ( c=0; c < size; c++ )
149 |     {
150 | 		crc = CRC_TABLE_CCITT16[ ( crc ^ *( data ++ )) & 0x00FF ] ^ ( crc >> 8 );
151 | 	}
152 |     return crc;
153 | }
154 | 
155 | 
156 | /**
157 |  * Calculates the CRC16 checksum of an array by using a table.
158 |  * The crc16 polynomial is 0x1021 ( x^16 + x^12 + x^5 + 1 ).
159 |  * This function takes 0xffff as initial value.
160 |  *
161 |  * Note: The checksum generated by this function is NOT according
162 |  * to CCITT standard!
163 |  *
164 |  * @param *data		Points to the byte array from which checksum should
165 |  * 					be calculated
166 |  * @param size		Size of the byte array
167 |  *
168 |  * @return CRC16 checksum
169 |  */
170 | 
171 | unsigned short checksum_crc16( unsigned char *data, unsigned int size )
172 | {
173 | 	return( checksum_update_crc16( data, size, 0xffff ) );
174 | }
175 | 
176 | //------------------------------------------------------------------------
177 | // Testing functions
178 | //------------------------------------------------------------------------
179 | 


--------------------------------------------------------------------------------
/wsg_50_driver/src/cmd.c:
--------------------------------------------------------------------------------
  1 | //======================================================================
  2 | /**
  3 |  *  @file
  4 |  *  cmd.c
  5 |  *
  6 |  *  @section cmd.c_general General file information
  7 |  *
  8 |  *  @brief
  9 |  *  Command abstraction layer
 10 |  *
 11 |  *  @author wolfer
 12 |  *  @date	20.07.2011
 13 |  *  
 14 |  *  
 15 |  *  @section cmd.c_copyright Copyright
 16 |  *  
 17 |  *  Copyright 2011 Weiss Robotics, D-71636 Ludwigsburg, Germany
 18 |  *  
 19 |  *  Redistribution and use in source and binary forms, with or without
 20 |  *  modification, are permitted provided that the following conditions are met:
 21 |  *
 22 |  *     * Redistributions of source code must retain the above copyright
 23 |  *       notice, this list of conditions and the following disclaimer.
 24 |  *     * Redistributions in binary form must reproduce the above copyright
 25 |  *       notice, this list of conditions and the following disclaimer in the
 26 |  *       documentation and/or other materials provided with the distribution.
 27 |  *     * Neither the name of the and Weiss Robotics GmbH nor the names of its 
 28 |  *       contributors may be used to endorse or promote products derived from
 29 |  *	 this software without specific prior written permission.
 30 |  *
 31 |  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 32 |  *  AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 33 |  *  IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 34 |  *  ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
 35 |  *  LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 36 |  *  CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 37 |  *  SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 38 |  *  INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 39 |  *  CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 40 |  *  ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 41 |  *  POSSIBILITY OF SUCH DAMAGE.
 42 |  *
 43 |  */
 44 | //======================================================================
 45 | 
 46 | 
 47 | 
 48 | //------------------------------------------------------------------------
 49 | // Includes
 50 | //------------------------------------------------------------------------
 51 | 
 52 | #include <stdio.h>
 53 | #include <stdlib.h>
 54 | #include <string.h>
 55 | #include <assert.h>
 56 | 
 57 | #include "wsg_50/common.h"
 58 | #include "wsg_50/msg.h"
 59 | #include "wsg_50/cmd.h"
 60 | 
 61 | #include "wsg_50/tcp.h"
 62 | #include "wsg_50/udp.h"
 63 | #include "wsg_50/serial.h"
 64 | 
 65 | 
 66 | //------------------------------------------------------------------------
 67 | // Local macros
 68 | //------------------------------------------------------------------------
 69 | 
 70 | 
 71 | //------------------------------------------------------------------------
 72 | // Typedefs, enums, structs
 73 | //------------------------------------------------------------------------
 74 | 
 75 | 
 76 | //------------------------------------------------------------------------
 77 | // Global variables
 78 | //------------------------------------------------------------------------
 79 | 
 80 | static bool connected = false;
 81 | 
 82 | 
 83 | //------------------------------------------------------------------------
 84 | // Unit testing
 85 | //------------------------------------------------------------------------
 86 | 
 87 | 
 88 | //------------------------------------------------------------------------
 89 | // Local function prototypes
 90 | //------------------------------------------------------------------------
 91 | 
 92 | 
 93 | //------------------------------------------------------------------------
 94 | // Function implementation
 95 | //------------------------------------------------------------------------
 96 | 
 97 | 
 98 | /**
 99 |  * Send command and wait for answer
100 |  *
101 |  * @param id		Command ID
102 |  * @param len		Payload length
103 |  * @param *payload	Payload data
104 |  * @param pending	Flag indicating whether CMD_PENDING
105 |  * 					is allowed return status
106 |  *
107 |  * @return Number of bytes received. -1 on error.
108 |  */
109 | 
110 | int cmd_submit( unsigned char id, unsigned char *payload, unsigned int len,
111 | 			    bool pending, unsigned char **response, unsigned int *response_len )
112 | {
113 | 	int res;
114 | 	status_t status;
115 | 
116 | 	// Assemble message struct
117 | 	msg_t msg =
118 | 	{
119 | 		.id = id,
120 | 		.len = len,
121 | 		.data = payload
122 | 	};
123 | 
124 | 	// Check if we're connected
125 | 	if ( !connected )
126 | 	{
127 | 		fprintf( stderr, "Interface not connected\n" );
128 | 		return -1;
129 | 	}
130 | 
131 | 	// Send command
132 | 	res = msg_send( &msg );
133 | 	if ( res < 0 )
134 | 	{
135 | 		fprintf( stderr, "Message send failed\n" );
136 | 		return -1;
137 | 	}
138 | 
139 | 	// Reuse message struct to receive response
140 | 	memset( &msg, 0, sizeof( msg ) );
141 | 
142 | 	// Receive response. Repeat if pending.
143 | 	do
144 | 	{
145 | 		// Free response
146 | 		msg_free( &msg );
147 | 
148 | 		// Receive response data
149 | 		res = msg_receive( &msg );
150 | 		if ( res < 0 )
151 | 		{
152 | 			fprintf( stderr, "Message receive failed\n" );
153 | 			return -1;
154 | 		}
155 | 
156 | 		// Check response ID
157 | 		if ( msg.id != id )
158 | 		{
159 | 			fprintf( stderr, "Response ID (%2x) does not match submitted command ID (%2x)\n", msg.id, id );
160 | 			return -1;
161 | 		}
162 | 
163 | 		if ( pending )
164 | 		{
165 | 			if ( msg.len < 2 )
166 | 			{
167 | 				fprintf( stderr, "No status code received\n" );
168 | 				return -1;
169 | 			}
170 | 
171 | 			status = (status_t) make_short( msg.data[0], msg.data[1] );
172 | 		}
173 | 	}
174 | 	while( pending && status == E_CMD_PENDING );
175 | 
176 | 
177 | 	// Return payload
178 | 	*response_len = msg.len;
179 | 	if ( msg.len > 0 ) *response = msg.data;
180 | 	else *response = 0;
181 | 
182 | 	return (int) msg.len;
183 | }
184 | 
185 | 
186 | /**
187 |  * Open TCP connection
188 |  *
189 |  * @param *addr				String containing IP address
190 |  * @param port				Port number (remote)
191 |  *
192 |  * @return 0 on success, else -1
193 |  */
194 | 
195 | int cmd_connect_tcp( const char *addr, unsigned short port )
196 | {
197 | 	int res;
198 | 	tcp_params_t params;
199 | 	const interface_t *iface;
200 | 
201 | 	// IP address string must be given
202 | 	if ( !addr ) return -1;
203 | 
204 | 	// If already connected, return error
205 | 	if ( connected ) return -1;
206 | 
207 | 	// Get interface with the given name
208 | 	iface = interface_get( "tcp" );
209 | 	if ( !iface ) return -1;
210 | 
211 | 	// Create parameter struct
212 | 	params.addr = str_to_ipaddr( addr );
213 | 	params.port = port;
214 | 
215 | 	// Open connection
216 | 	res = msg_open( iface, &params );
217 | 	if ( res < 0 ) return -1;
218 | 
219 | 	// Set connected flag
220 | 	connected = true;
221 | 
222 | 	//printf( "TCP connection established. \n" );
223 | 
224 | 	return 0;
225 | }
226 | 
227 | 
228 | /**
229 |  * Open up UDP connection
230 |  *
231 |  * @param local_port		Local port number (for answer)
232 |  * @param *addr				String containing IP address
233 |  * @param remote_port		Remote port number
234 |  *
235 |  * @return 0 on success, else -1
236 |  */
237 | 
238 | int cmd_connect_udp( unsigned short local_port, const char *addr, unsigned short remote_port )
239 | {
240 | 	int res;
241 | 	udp_params_t params;
242 | 	const interface_t *iface;
243 | 
244 | 	// IP address string must be given
245 | 	if ( !addr ) return -1;
246 | 
247 | 	// If already connected, return error
248 | 	if ( connected ) return -1;
249 | 
250 | 	// Get interface with the given name
251 | 	iface = interface_get( "udp" );
252 | 	if ( !iface ) return -1;
253 | 
254 | 	// Create parameter struct
255 | 	params.addr = str_to_ipaddr( addr );
256 | 	params.local_port = local_port;
257 | 	params.remote_port = remote_port;
258 | 
259 | 	// Open connection
260 | 	res = msg_open( iface, &params );
261 | 	if ( res < 0 ) return -1;
262 | 
263 | 	// Set connected flag
264 | 	connected = true;
265 | 
266 | 	printf( "UDP connection established\n" );
267 | 
268 | 	return 0;
269 | }
270 | 
271 | 
272 | /**
273 |  * Open up serial connection
274 |  *
275 |  * @param *device		Device string
276 |  *
277 |  * @return 0 on success, else -1
278 |  */
279 | 
280 | int cmd_connect_serial( const char *device, unsigned int bitrate )
281 | {
282 | 	int res;
283 | 	ser_params_t params;
284 | 	const interface_t *iface;
285 | 
286 | 	// Device parameter must be given
287 | 	if ( !device || bitrate == 0 ) return -1;
288 | 
289 | 	// Set connection parameters
290 | 	params.device = device;
291 | 	params.bitrate = bitrate;
292 | 
293 | 	// If already connected, return error
294 | 	if ( connected ) return -1;
295 | 
296 | 	// Get interface with the given name
297 | 	iface = interface_get( "serial" );
298 | 	if ( !iface ) return -1;
299 | 
300 | 	// Open connection
301 | 	res = msg_open( iface, (void *) &params );
302 | 	if ( res < 0 ) return -1;
303 | 
304 | 	// Set connected flag
305 | 	connected = true;
306 | 
307 | 	printf( "Serial connection established\n" );
308 | 
309 | 	return 0;
310 | }
311 | 
312 | 
313 | /**
314 |  * Disconnect
315 |  */
316 | 
317 | void cmd_disconnect( void )
318 | {
319 | 	status_t status;
320 | 	int res;
321 | 	unsigned char *resp;
322 | 	unsigned int resp_len;
323 | 
324 | 	printf( "Closing connection\n" );
325 | 
326 | 	res = cmd_submit( 0x07, NULL, 0, false, &resp, &resp_len );
327 | 	if ( res != 2 ) printf( "Disconnect announcement failed: Response payload length doesn't match (is %d, expected 2)\n", res );
328 | 	else
329 | 	{
330 | 		// Check response status
331 | 		status = cmd_get_response_status( resp );
332 | 		if ( status != E_SUCCESS ) printf( "Command ANNOUNCE DISCONNECT not successful: %s\n", status_to_str( status ) );
333 | 	}
334 | 
335 | 	if ( res > 0 ) free( resp );
336 | 
337 | 	msg_close();
338 | }
339 | 
340 | 
341 | /**
342 |  * Get connection state
343 |  *
344 |  * @return true if the command interface is connected, else false
345 |  */
346 | 
347 | bool cmd_is_connected( void )
348 | {
349 | 	return connected;
350 | }
351 | 
352 | 
353 | /**
354 |  * Get status code from response data
355 |  *
356 |  * @return Status code
357 |  */
358 | 
359 | status_t cmd_get_response_status( unsigned char *response )
360 | {
361 | 	status_t status;
362 | 
363 | 	assert( response != NULL );
364 | 
365 | 	status = (status_t) make_short( response[0], response[1] );
366 | 
367 | 	return status;
368 | }
369 | 
370 | //------------------------------------------------------------------------
371 | // Testing functions
372 | //------------------------------------------------------------------------
373 | 


--------------------------------------------------------------------------------
/wsg_50_driver/src/common.cpp:
--------------------------------------------------------------------------------
  1 | //======================================================================
  2 | /**
  3 |  *  @file
  4 |  *  common.c
  5 |  *
  6 |  *  @section common.c_general General file information
  7 |  *
  8 |  *  @brief
  9 |  *  
 10 |  *
 11 |  *  @author wolfer
 12 |  *  @date	19.07.2011
 13 |  *  
 14 |  *  
 15 |  *  @section common.c_copyright Copyright
 16 |  *  
 17 |  *  Copyright 2011 Weiss Robotics, D-71636 Ludwigsburg, Germany
 18 |  *  
 19 |  *  Redistribution and use in source and binary forms, with or without
 20 |  *  modification, are permitted provided that the following conditions are met:
 21 |  *
 22 |  *     * Redistributions of source code must retain the above copyright
 23 |  *       notice, this list of conditions and the following disclaimer.
 24 |  *     * Redistributions in binary form must reproduce the above copyright
 25 |  *       notice, this list of conditions and the following disclaimer in the
 26 |  *       documentation and/or other materials provided with the distribution.
 27 |  *     * Neither the name of the and Weiss Robotics GmbH nor the names of its 
 28 |  *       contributors may be used to endorse or promote products derived from
 29 |  *	 this software without specific prior written permission.
 30 |  *
 31 |  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 32 |  *  AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 33 |  *  IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 34 |  *  ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
 35 |  *  LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 36 |  *  CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 37 |  *  SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 38 |  *  INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 39 |  *  CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 40 |  *  ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 41 |  *  POSSIBILITY OF SUCH DAMAGE.
 42 |  *
 43 |  */
 44 | //======================================================================
 45 | 
 46 | 
 47 | 
 48 | //------------------------------------------------------------------------
 49 | // Includes
 50 | //------------------------------------------------------------------------
 51 | 
 52 | #include <stdio.h>
 53 | #include <stdlib.h>
 54 | #include <string.h>
 55 | 
 56 | #include "wsg_50/common.h"
 57 | 
 58 | 
 59 | //------------------------------------------------------------------------
 60 | // Local macros
 61 | //------------------------------------------------------------------------
 62 | 
 63 | 
 64 | //------------------------------------------------------------------------
 65 | // Typedefs, enums, structs
 66 | //------------------------------------------------------------------------
 67 | 
 68 | 
 69 | //------------------------------------------------------------------------
 70 | // Global variables
 71 | //------------------------------------------------------------------------
 72 | 
 73 | 
 74 | //------------------------------------------------------------------------
 75 | // Unit testing
 76 | //------------------------------------------------------------------------
 77 | 
 78 | 
 79 | //------------------------------------------------------------------------
 80 | // Local function prototypes
 81 | //------------------------------------------------------------------------
 82 | 
 83 | 
 84 | //------------------------------------------------------------------------
 85 | // Function implementation
 86 | //------------------------------------------------------------------------
 87 | 
 88 | /**
 89 |  * Convert IP address string to IP address type
 90 |  *
 91 |  * @param *str		String containing IP address
 92 |  *
 93 |  * @return IP address type
 94 |  */
 95 | 
 96 | ip_addr_t str_to_ipaddr( const char *str )
 97 | {
 98 | 	unsigned int i, res;
 99 | 	unsigned int buf[4];
100 | 	ip_addr_t addr = 0;
101 | 	res = sscanf( str, "%d.%d.%d.%d", &buf[3], &buf[2], &buf[1], &buf[0] );
102 | 	if ( res != 4 ) return( 0 );
103 | 	for ( i = 0; i < 4; i++ )
104 | 	{
105 | 		if ( buf[i] > 255 ) return( 0 );
106 | 		addr <<= 8;
107 | 		addr |= (int) buf[i];
108 | 	}
109 | 	return( addr );
110 | }
111 | 
112 | 
113 | /**
114 |  * Convert status code to string
115 |  *
116 |  * @param status	Status code
117 |  *
118 |  * @return Status string
119 |  */
120 | 
121 | const char * status_to_str( status_t status )
122 | {
123 | 	switch( status )
124 | 	{
125 | 		case E_SUCCESS:					return( "No error" );
126 | 		case E_NOT_AVAILABLE:			return( "Service or data is not available" );
127 | 		case E_NO_SENSOR:				return( "No sensor connected" );
128 | 		case E_NOT_INITIALIZED:			return( "The device is not initialized" );
129 | 		case E_ALREADY_RUNNING:			return( "Service is already running" );
130 | 		case E_FEATURE_NOT_SUPPORTED:	return( "The requested feature is not supported" );
131 | 		case E_INCONSISTENT_DATA:		return( "One or more dependent parameters mismatch" );
132 | 		case E_TIMEOUT:					return( "Timeout error" );
133 | 		case E_READ_ERROR:				return( "Error while reading from a device" );
134 | 		case E_WRITE_ERROR:				return( "Error while writing to a device" );
135 | 		case E_INSUFFICIENT_RESOURCES:	return( "No memory available" );
136 | 		case E_CHECKSUM_ERROR:			return( "Checksum error" );
137 | 		case E_NO_PARAM_EXPECTED:		return( "No parameters expected" );
138 | 		case E_NOT_ENOUGH_PARAMS:		return( "Not enough parameters" );
139 | 		case E_CMD_UNKNOWN:				return( "Unknown command" );
140 | 		case E_CMD_FORMAT_ERROR:		return( "Command format error" );
141 | 		case E_ACCESS_DENIED:			return( "Access denied" );
142 | 		case E_ALREADY_OPEN:			return( "Interface already open" );
143 | 		case E_CMD_FAILED:				return( "Command failed" );
144 | 		case E_CMD_ABORTED:				return( "Command aborted" );
145 | 		case E_INVALID_HANDLE:			return( "Invalid handle" );
146 | 		case E_NOT_FOUND:				return( "Device not found" );
147 | 		case E_NOT_OPEN:				return( "Device not open" );
148 | 		case E_IO_ERROR:				return( "General I/O-Error" );
149 | 		case E_INVALID_PARAMETER:		return( "Invalid parameter" );
150 | 		case E_INDEX_OUT_OF_BOUNDS:		return( "Index out of bounds" );
151 | 		case E_CMD_PENDING:				return( "Command is pending..." );
152 | 		case E_OVERRUN:					return( "Data overrun" );
153 | 		case E_RANGE_ERROR:				return( "Value out of range" );
154 | 		case E_AXIS_BLOCKED:			return( "Axis is blocked" );
155 | 		case E_FILE_EXISTS:				return( "File already exists" );
156 | 		default:						return( "Internal error. Unknown error code." );
157 | 	}
158 | }
159 | 
160 | 
161 | /**
162 |  * Quit program for the given reason
163 |  *
164 |  * @param *reason	String telling why we're quitting
165 |  */
166 | 
167 | void quit( const char *reason )
168 | {
169 | 	if ( reason ) fprintf( stderr, "%s\n", reason );
170 | 	exit(1);
171 | }
172 | 
173 | 
174 | const char * getStateValues( unsigned char *b ){
175 | 
176 | 	/*
177 | 	unsigned char aux[4]; 
178 | 
179 | 	
180 | 	aux[0] = b[0];
181 | 	aux[1] = b[1];
182 | 	aux[2] = b[2];
183 | 	aux[3] = b[3];
184 | 	
185 | 	dbgPrint("Dins de getStateValues.\n");
186 | 	dbgPrint("b[2] = 0x%x\n", b[2]);
187 | 	dbgPrint("b[3] = 0x%x\n", b[3]);
188 | 	dbgPrint("b[4] = 0x%x\n", b[4]);
189 | 	dbgPrint("b[5] = 0x%x\n", b[5]);
190 | 	*/
191 | 
192 | 	char resp[1024] = "| ";
193 | 
194 | 	if (b[2] & 0x1){	// D0 ==> LSB
195 | 		//dbgPrint("Fingers Referenced.\n");
196 | 		char aux0[21] = "Fingers Referenced |";
197 | 		strcat(resp, aux0);
198 | 	}
199 | 	if (b[2] & 0x2){  // D1
200 | 		//dbgPrint("The Fingers are currently moving.\n");
201 | 		char aux1[36]=" The Fingers are currently moving |";
202 | 		strcat(resp, aux1);
203 | 	}
204 | 	if (b[2] & 0x4){  // D2
205 | 		//dbgPrint("Axis is blocked in negative moving direction.\n");
206 | 		char aux2[48] =" Axis is blocked in negative moving direction |";
207 | 		strcat(resp, aux2);
208 | 	}
209 |      	if (b[2] & 0x8){  // D3
210 | 		//dbgPrint("Axis is blocked in positive moving direction.\n");
211 | 		char aux3[48] =" Axis is blocked in positive moving direction |";
212 | 		strcat(resp, aux3);
213 | 	}
214 | 	if (b[2] & 0x10){ // D4
215 | 		//dbgPrint("Negative direction soft limit reached.\n");
216 | 		char aux4[42] = " Negative direction soft limit reached |";
217 | 		strcat(resp, aux4);
218 | 	}
219 | 	if (b[2] & 0x20){ // D5
220 | 		//dbgPrint("Positive direction soft limit reached.\n");
221 | 		char aux5[42] = " Positive direction soft limit reached |";
222 | 		strcat(resp, aux5);
223 | 	}
224 | 	if (b[2] & 0x40){ // D6
225 | 		//dbgPrint("Axis Stopped.\n");
226 | 		char aux6[18] = " Axis Stopped |";
227 | 		strcat(resp, aux6);
228 | 	}
229 | 	if (b[2] & 0x80){ // D7
230 | 		//dbgPrint("Target Pos reached.\n");
231 | 		char aux7[22] = " Target Pos reached |";
232 | 		strcat(resp, aux7);
233 | 	}
234 | 
235 | 	if (b[3] & 0x1){ // D8
236 | 		//dbgPrint("Overdrive Mode.\n");
237 | 		char aux8[18] = " Overdrive Mode |";
238 | 		strcat(resp, aux8);
239 | 	}
240 | 	if (b[3] & 0x10){ // D12
241 | 		//dbgPrint("Fast Stop.\n");
242 | 		char aux12[13] = " Fast Stop |";
243 | 		strcat(resp, aux12);
244 | 	}
245 | 	if (b[3] & 0x20){ // D13
246 | 		//dbgPrint("Temperature Warning.\n");
247 | 		char aux13[23] = " Temperature Warning |";
248 | 		strcat(resp,aux13);
249 | 	}	
250 | 	if (b[3] & 0x40){ // D14
251 | 		//dbgPrint("Temperature Error.\n");
252 | 		char aux14[21]= " Temperature Error |";
253 | 		strcat(resp, aux14);
254 | 	}	
255 | 	if (b[3] & 0x80){ // D15
256 | 		//dbgPrint("Power Error.\n");
257 | 		char aux15[15]= " Power Error |";
258 | 		strcat(resp, aux15);
259 | 	}
260 | 
261 | 	if (b[4] & 0x1){  // D16
262 | 		//dbgPrint("Engine Current Error.\n");
263 | 		char aux16[24]= " Engine Current Error |";
264 | 		strcat(resp, aux16);
265 | 	}
266 | 	if (b[4] & 0x2){  // D17
267 | 		//dbgPrint("Finger Fault.\n");
268 | 		char aux17[16] = " Finger Fault |";
269 | 		strcat(resp, aux17);
270 | 	}
271 | 	if (b[4] & 0x4){  // D18
272 | 		//dbgPrint("Command Error.\n");
273 | 		char aux18[17] = " Command Error |";
274 | 		strcat(resp, aux18);
275 | 	}
276 |      	if (b[4] & 0x8){  // D19
277 | 		//dbgPrint("A script is currently running.\n");
278 | 		char aux19[33] = " A script is currently running |";
279 | 		strcat(resp, aux19);
280 | 	}
281 | 	if (b[4] & 0x10){ // D20
282 | 		//dbgPrint("Script Error.\n");
283 | 		char aux20[16] = " Script Error |";
284 | 		strcat(resp, aux20);
285 | 	}
286 | 
287 | 	// [D21 - D31] RESERVED
288 | 	
289 | 	// D31 ==> MSB
290 | 
291 | 	//dbgPrint("%s\n", resp);
292 | 	return resp;
293 | }
294 | 
295 | 
296 | //------------------------------------------------------------------------
297 | // Testing functions
298 | //------------------------------------------------------------------------
299 | 


--------------------------------------------------------------------------------
/wsg_50_driver/src/functions.cpp:
--------------------------------------------------------------------------------
  1 | //======================================================================
  2 | /**
  3 |  *  @file
  4 |  *  functions.c
  5 |  *
  6 |  *  @section functions.c_general General file information
  7 |  *
  8 |  *  @brief
  9 |  *  
 10 |  *
 11 |  *  @author Marc
 12 |  *  @date   06.06.2012
 13 |  *  
 14 |  *  
 15 |  *  @section functions.c_copyright Copyright
 16 |  *  
 17 |  *  Copyright 2012 Robotnik Automation, SLL
 18 |  *  
 19 |  *  Redistribution and use in source and binary forms, with or without
 20 |  *  modification, are permitted provided that the following conditions are met:
 21 |  *
 22 |  *     * Redistributions of source code must retain the above copyright
 23 |  *       notice, this list of conditions and the following disclaimer.
 24 |  *     * Redistributions in binary form must reproduce the above copyright
 25 |  *       notice, this list of conditions and the following disclaimer in the
 26 |  *       documentation and/or other materials provided with the distribution.
 27 |  *     * Neither the name of the and Weiss Robotics GmbH nor the names of its 
 28 |  *       contributors may be used to endorse or promote products derived from
 29 |  *	 this software without specific prior written permission.
 30 |  *
 31 |  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 32 |  *  AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 33 |  *  IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 34 |  *  ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
 35 |  *  LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 36 |  *  CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 37 |  *  SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 38 |  *  INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 39 |  *  CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 40 |  *  ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 41 |  *  POSSIBILITY OF SUCH DAMAGE.
 42 |  *
 43 |  */
 44 | //======================================================================
 45 | 
 46 | 
 47 | //------------------------------------------------------------------------
 48 | // Includes
 49 | //------------------------------------------------------------------------
 50 | 
 51 | #include <stdio.h>
 52 | #include <stdlib.h>
 53 | #include <string.h>
 54 | #include <cmath>
 55 | #include <string>
 56 | 
 57 | #include "wsg_50/common.h"
 58 | #include "wsg_50/cmd.h"
 59 | #include "wsg_50/msg.h"
 60 | #include "wsg_50/functions.h"
 61 | 
 62 | //------------------------------------------------------------------------
 63 | // Support functions
 64 | //------------------------------------------------------------------------
 65 | 
 66 | float convert(unsigned char *b){
 67 | 	float tmp;
 68 |   	unsigned int src = 0;
 69 | 
 70 | 	/*
 71 | 	dbgPrint("b[3]=%x\n", b[3]);
 72 | 	dbgPrint("b[2]=%x\n", b[2]);
 73 | 	dbgPrint("b[1]=%x\n", b[1]);
 74 | 	dbgPrint("b[0]=%x\n", b[0]);
 75 | 	*/
 76 | 
 77 |   	src = b[3] * 16777216 + b[2] * 65536 + b[1] * 256 + b[0];
 78 | 
 79 |   	memcpy(&tmp, &src, sizeof tmp);
 80 |   	//printf("Converted value: %f \n", tmp);
 81 | 
 82 | 	return tmp;
 83 | }
 84 | 
 85 | 
 86 | //------------------------------------------------------------------------
 87 | // Function implementation
 88 | //------------------------------------------------------------------------
 89 | 
 90 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 91 | // Note: Argument values that are outside the gripper’s physical limits are clamped to the highest/lowest available value. //
 92 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 93 | 
 94 | /////////////////////////
 95 | // ACTUATION FUNCTIONS //
 96 | /////////////////////////
 97 | 
 98 | 
 99 | int homing( void )
100 | {
101 | 	status_t status;
102 | 	int res;
103 | 	unsigned char payload[1];
104 | 	unsigned char *resp;
105 | 	unsigned int resp_len;
106 | 
107 | 	// Set flags: Homing direction (0: default, 1: widthitive movement, 2: negative movement).
108 | 	payload[0] = 0x00;
109 | 
110 | 	// Submit command and wait for response. Push result to stack.
111 | 	res = cmd_submit( 0x20, payload, 1, true, &resp, &resp_len );
112 | 	if ( res != 2 )
113 | 	{
114 | 		dbgPrint( "Response payload length doesn't match (is %d, expected 2)\n", res );
115 | 		if ( res > 0 ) free( resp );
116 | 		return 0;
117 | 	}
118 | 
119 | 	//dbgPrint("&resp: %s\n", resp);
120 | 	//dbgPrint("&resp_len: %d\n", resp_len);
121 | 
122 | 	// Check response status
123 | 	status = cmd_get_response_status( resp );
124 | 	free( resp );
125 | 	if ( status != E_SUCCESS )
126 | 	{
127 | 		dbgPrint( "Command HOMING not successful: %s\n", status_to_str( status ) );
128 | 		return -1;
129 | 	}
130 | 
131 | 	return 0;
132 | }
133 | 
134 | 
135 | /** \brief  Send move command (0x21) to gripper
136 |  *  \param  ignore_response Do not read back response from gripper. (Must be read elsewhere, for auto update.)
137 |  */
138 | int move( float width, float speed, bool stop_on_block, bool ignore_response)
139 | {
140 | 
141 | 	status_t status;
142 | 	int res;
143 | 	unsigned char payload[9];
144 | 	unsigned char *resp;
145 | 	unsigned int resp_len;
146 | 
147 | 	// Set flags: Absolute movement (bit 0 is 0), stop on block (bit 1 is 1).
148 | 	payload[0] = 0x00;
149 | 	if (stop_on_block) payload[0] |= 0x02;
150 | 
151 | 	// Copy target width and speed
152 | 	memcpy( &payload[1], &width, sizeof( float ) );
153 | 	memcpy( &payload[5], &speed, sizeof( float ) );
154 | 
155 |     if (!ignore_response) {
156 |         // Submit command and wait for response. Push result to stack.
157 |         res = cmd_submit( 0x21, payload, 9, true, &resp, &resp_len );
158 |         if ( res != 2 )
159 |         {
160 |             dbgPrint( "Response payload length doesn't match (is %d, expected 2)\n", res );
161 |             if ( res > 0 ) free( resp );
162 |             return 0;
163 |         }
164 | 
165 |         // Check response status
166 |         status = cmd_get_response_status( resp );
167 |         free( resp );
168 |         if ( status != E_SUCCESS )
169 |         {
170 |             dbgPrint( "Command MOVE not successful: %s\n", status_to_str( status ) );
171 |             return -1;
172 |         }
173 |     } else {
174 |         // Submit command, do not wait for response
175 |         msg_t msg;
176 |         msg.id = 0x21; msg.len = 9; msg.data = &payload[0];
177 |         res = msg_send(&msg);
178 |         if (res <= 0) {
179 |             dbgPrint("Failed to send command MOVE\n");
180 |             return -1;
181 |         }
182 |     }
183 | 
184 | 	return 0;
185 | }
186 | 
187 | 
188 | int stop( bool ignore_response )
189 | {
190 | 	status_t status;
191 | 	int res;
192 | 	unsigned char payload[1];
193 | 	unsigned char *resp;
194 | 	unsigned int resp_len;
195 | 
196 | 	//payload[0] = 0x00;
197 | 
198 |     if (!ignore_response) {
199 |         // Submit command and wait for response. Push result to stack.
200 |         res = cmd_submit( 0x22, payload, 0, true, &resp, &resp_len );
201 |         if ( res != 2 )
202 |         {
203 |             dbgPrint( "Response payload length doesn't match (is %d, expected 2)\n", res );
204 |             if ( res > 0 ) free( resp );
205 |             return 0;
206 |         }
207 | 
208 |         // Check response status
209 |         status = cmd_get_response_status( resp );
210 |         free( resp );
211 |         if ( status != E_SUCCESS )
212 |         {
213 |             dbgPrint( "Command STOP not successful: %s\n", status_to_str( status ) );
214 |             return -1;
215 |         }
216 |     } else {
217 |         // Submit command, do not wait for response
218 |         msg_t msg;
219 |         msg.id = 0x22; msg.len = 0; msg.data = &payload[0];
220 |         res = msg_send(&msg);
221 |         if (res <= 0) {
222 |             dbgPrint("Failed to send command STOP\n");
223 |             return -1;
224 |         }
225 |     }
226 | 
227 |     return 0;
228 | }
229 | 
230 | 
231 | int ack_fault( void )
232 | {
233 | 	status_t status;
234 | 	int res;
235 | 	unsigned char payload[3];
236 | 	unsigned char *resp;
237 | 	unsigned int resp_len;
238 | 
239 | 	payload[0] = 0x61;  //MBJ: Està ben enviat, si es posa alrevés no torna error en terminal però si que es posa roig el LED
240 | 	payload[1] = 0x63;
241 | 	payload[2] = 0x6B;
242 | 
243 | 	// Submit command and wait for response. Push result to stack.
244 | 	res = cmd_submit( 0x24, payload, 3, true, &resp, &resp_len );
245 | 	if ( res != 2 )
246 | 	{
247 | 		dbgPrint( "Response payload length doesn't match (is %d, expected 2)\n", res );
248 | 		if ( res > 0 ) free( resp );
249 | 		return 0;
250 | 	}
251 | 
252 | 
253 | 	// Check response status
254 | 	status = cmd_get_response_status( resp );
255 | 	free( resp );
256 | 	if ( status != E_SUCCESS )
257 | 	{
258 | 		dbgPrint( "Command ACK not successful: %s\n", status_to_str( status ) );
259 | 		return -1;
260 | 	}
261 | 
262 | 	return 0;
263 | }
264 | 
265 | 
266 | int grasp( float objWidth, float speed )
267 | {
268 | 	status_t status;
269 | 	int res;
270 | 	unsigned char payload[8];
271 | 	unsigned char *resp;
272 | 	unsigned int resp_len;
273 | 
274 | 	// Copy part width and speed
275 | 	memcpy( &payload[0], &objWidth, sizeof( float ) );
276 | 	memcpy( &payload[4], &speed, sizeof( float ) );
277 | 
278 | 	// Submit command and wait for response. Push result to stack.
279 | 	res = cmd_submit( 0x25, payload, 8, true, &resp, &resp_len );
280 | 	if ( res != 2 )
281 | 	{
282 | 		dbgPrint( "Response payload length doesn't match (is %d, expected 2)\n", res );
283 | 		if ( res > 0 ) free( resp );
284 | 		return 0;
285 | 	}
286 | 
287 | 	// Check response status
288 | 	status = cmd_get_response_status( resp );
289 | 	free( resp );
290 | 	if ( status != E_SUCCESS )
291 | 	{
292 | 		dbgPrint( "Command GRASP not successful: %s\n", status_to_str( status ) );
293 | 		return -1;
294 | 	}
295 | 
296 | 	return( 0 );
297 | }
298 | 
299 | 
300 | int release( float width, float speed )
301 | {
302 | 	status_t status;
303 | 	int res;
304 | 	unsigned char payload[8];
305 | 	unsigned char *resp;
306 | 	unsigned int resp_len;
307 | 
308 | 	// Copy part width and speed
309 | 	memcpy( &payload[0], &width, sizeof( float ) );
310 | 	memcpy( &payload[4], &speed, sizeof( float ) );
311 | 
312 | 	// Submit command and wait for response. Push result to stack.
313 | 	res = cmd_submit( 0x26, payload, 8, true, &resp, &resp_len );
314 | 	if ( res != 2 )
315 | 	{
316 | 		dbgPrint( "Response payload length doesn't match (is %d, expected 2)\n", res );
317 | 		if ( res > 0 ) free( resp );
318 | 		return -1;
319 | 	}
320 | 
321 | 	// Check response status
322 | 	status = cmd_get_response_status( resp );
323 | 	free( resp );
324 | 	if ( status != E_SUCCESS )
325 | 	{
326 | 		dbgPrint( "Command RELEASE not successful: %s\n", status_to_str( status ) );
327 | 		return -1;
328 | 	}
329 | 
330 | 	return 0;
331 | }
332 | 
333 | 
334 | // Custom script: Command-and-measure
335 | // cmd_type:	0 - read only; 1 - position control; 2 - speed control
336 | int script_measure_move (unsigned char cmd_type, float cmd_width, float cmd_speed, gripper_response & info)
337 | {
338 | 	status_t status;
339 | 	int res;
340 | 	const unsigned char CMD_CUSTOM = 0xB0;
341 | 	unsigned char payload[9];
342 | 	unsigned char *resp;
343 | 	unsigned int resp_len;
344 | 
345 | 	// Custom payload format:
346 | 	// 0:	Unused
347 | 	// 1:	float, target width, used for 0xB1 command
348 | 	// 5:	float, target speed, used for 0xB1 and 0xB2 command
349 | 	payload[0] = 0x00;
350 | 	memcpy(&payload[1], &cmd_width, sizeof(float));
351 | 	memcpy(&payload[5], &cmd_speed, sizeof(float));
352 | 
353 | 	// Submit command and process result
354 | 	res = cmd_submit(CMD_CUSTOM + cmd_type, payload, 9, true, &resp, &resp_len );
355 | 	try {
356 | 		if (res < 2)
357 | 			throw std::string("Invalid Response");
358 | 		status = cmd_get_response_status(resp);
359 | 		if (status == E_CMD_UNKNOWN)
360 | 			throw std::string("Command unknown - make sure script is running");
361 | 		if (status != E_SUCCESS)
362 | 			throw std::string("Command failed");
363 | 		if (res != 23)
364 | 			throw std::string("Response payload incorrect (" + std::to_string(res) + ")");
365 | 
366 | 		// Extract data from response
367 | 		int off=2;
368 | 		unsigned char resp_state[6] = {0,0,0,0,0,0};
369 | 		resp_state[2] = resp[2];
370 | 		info.state = resp[2];					 off+=1;
371 | 		info.state_text = std::string(getStateValues(resp_state));
372 | 		info.position = convert(&resp[off]);     off+=4;
373 | 		info.speed = convert(&resp[off]);        off+=4;
374 | 		info.f_motor = convert(&resp[off]);      off+=4;
375 | 		info.f_finger0 = convert(&resp[off]);    off+=4;
376 | 		info.f_finger1 = convert(&resp[off]);    off+=4;
377 | 
378 | 
379 | 		info.ismoving = (info.state & 0x02/*fingers mnoving*/) != 0;
380 | 		// only in position mode; cannot determine reliably for velocity mode
381 | 		// 0x40 /* axis stopped */
382 | 
383 | 		if (0)
384 | 			printf("Received: %02X, %6.2f,%6.2f,%6.2f,%6.2f,%6.2f\n  %s\n",
385 | 				info.state, info.position, info.speed, info.f_motor, info.f_finger0, info.f_finger1,
386 | 				info.state_text.c_str());
387 | 
388 | 	} catch (std::string msg) {
389 | 		msg = "measure_move: " + msg + "\n";
390 |         dbgPrint ("%s", msg.c_str());
391 | 		if (res > 0) free(resp);
392 | 		return 0;
393 | 	}
394 | 
395 | 
396 | 	free( resp );
397 | 	return 1;
398 | }
399 | 
400 | 
401 | 
402 | ///////////////////
403 | // SET FUNCTIONS //
404 | ///////////////////
405 | 
406 | 
407 | int setAcceleration( float acc )
408 | {
409 | 	status_t status;
410 | 	int res;
411 | 	unsigned char payload[4];
412 | 	unsigned char *resp;
413 | 	unsigned int resp_len;
414 | 
415 | 	// Copy target width and speed
416 | 	memcpy( &payload[0], &acc, sizeof( float ) );
417 | 
418 | 	// Submit command and wait for response. Push result to stack.
419 | 	res = cmd_submit( 0x30, payload, 4, true, &resp, &resp_len );
420 | 	if ( res != 2 )
421 | 	{
422 | 		dbgPrint( "Response payload length doesn't match (is %d, expected 2)\n", res );
423 | 		if ( res > 0 ) free( resp );
424 | 		return 0;
425 | 	}
426 | 
427 | 	// Check response status
428 | 	status = cmd_get_response_status( resp );
429 | 	free( resp );
430 | 	if ( status != E_SUCCESS )
431 | 	{
432 | 		dbgPrint( "Command SET ACCELERATION not successful: %s\n", status_to_str( status ) );
433 | 		return -1;
434 | 	}
435 | 
436 | 	return 0;
437 | }
438 | 
439 | int setGraspingForceLimit( float force )
440 | {
441 | 	status_t status;
442 | 	int res;
443 | 	unsigned char payload[4];
444 | 	unsigned char *resp;
445 | 	unsigned int resp_len;
446 | 
447 | 	// Copy target width and speed
448 | 	memcpy( &payload[0], &force, sizeof( float ) );
449 | 
450 | 	// Submit command and wait for response. Push result to stack.
451 | 	res = cmd_submit( 0x32, payload, 4, true, &resp, &resp_len );
452 | 	if ( res != 2 )
453 | 	{
454 | 		dbgPrint( "Response payload length doesn't match (is %d, expected 2)\n", res );
455 | 		if ( res > 0 ) free( resp );
456 | 		return 0;
457 | 	}
458 | 
459 | 	// Check response status
460 | 	status = cmd_get_response_status( resp );
461 | 	free( resp );
462 | 	if ( status != E_SUCCESS )
463 | 	{
464 | 		dbgPrint( "Command SET GRASPING FORCE LIMIT not successful: %s\n", status_to_str( status ) );
465 | 		return -1;
466 | 	}
467 | 
468 | 	return 0;
469 | }
470 | 
471 | 
472 | int doTare( void )
473 | {
474 |     status_t status;
475 |     int res;
476 |     unsigned char payload[3];
477 |     unsigned char *resp;
478 |     unsigned int resp_len;
479 | 
480 |     // Submit command and wait for response. Push result to stack.
481 |     res = cmd_submit( 0x38, payload, 0, true, &resp, &resp_len );
482 |     if ( res != 2 )
483 |     {
484 |         dbgPrint( "Response payload length doesn't match (is %d, expected 2)\n", res );
485 |         if ( res > 0 ) free( resp );
486 |         return 0;
487 |     }
488 | 
489 | 
490 |     // Check response status
491 |     status = cmd_get_response_status( resp );
492 |     free( resp );
493 |     if ( status != E_SUCCESS )
494 |     {
495 |         dbgPrint( "Command TARE not successful: %s\n", status_to_str( status ) );
496 |         return -1;
497 |     }
498 | 
499 |     return 0;
500 | }
501 | 
502 | 
503 | ///////////////////
504 | // GET FUNCTIONS //
505 | ///////////////////
506 | 
507 | 
508 | const char * systemState( void ) 
509 | {
510 | 	status_t status;
511 | 	int res;
512 | 	unsigned char payload[3];
513 | 	unsigned char *resp;
514 | 	unsigned int resp_len;
515 | 
516 | 	// Don't use automatic update, so the payload bytes are 0.
517 | 	memset( payload, 0, 3 );
518 | 
519 | 	// Submit command and wait for response. Expecting exactly 4 bytes response payload.
520 | 	res = cmd_submit( 0x40, payload, 3, false, &resp, &resp_len );
521 | 	if ( res != 6 )
522 | 	{
523 | 		dbgPrint( "Response payload length doesn't match (is %d, expected 6)\n", res );
524 | 		if ( res > 0 ) free( resp );
525 | 		return 0;
526 | 	}
527 | 
528 | 	// Check response status
529 | 	status = cmd_get_response_status( resp );
530 | 
531 | 	/*
532 | 	dbgPrint("LSB -> resp[0]: %x\n", resp[2]);
533 | 	dbgPrint("       resp[1]: %x\n", resp[3]);
534 | 	dbgPrint("       resp[2]: %x\n", resp[4]);
535 | 	dbgPrint("MSB -> resp[3]: %x\n", resp[5]);
536 | 	*/
537 | 
538 | 	return getStateValues(resp);
539 | 
540 | 	if ( status != E_SUCCESS )
541 | 	{
542 | 		dbgPrint( "Command GET SYSTEM STATE not successful: %s\n", status_to_str( status ) );
543 | 		free( resp );
544 | 		return 0;
545 | 	}
546 | 
547 | 	free( resp );
548 | 
549 |     return 0;
550 | 
551 | 	//return (int) resp[2]; MBJ
552 | }
553 | 
554 | 
555 | int graspingState( void )
556 | {
557 | 	status_t status;
558 | 	int res;
559 | 	unsigned char payload[3];
560 | 	unsigned char *resp;
561 | 	unsigned int resp_len;
562 | 
563 | 	// Don't use automatic update, so the payload bytes are 0.
564 | 	memset( payload, 0, 3 );
565 | 
566 | 	// Submit command and wait for response. Expecting exactly 4 bytes response payload.
567 | 	res = cmd_submit( 0x41, payload, 3, false, &resp, &resp_len );
568 | 	if ( res != 3 )
569 | 	{
570 | 		dbgPrint( "Response payload length doesn't match (is %d, expected 3)\n", res );
571 | 		if ( res > 0 ) free( resp );
572 | 		return 0;
573 | 	}
574 | 
575 | 	// Check response status
576 | 	status = cmd_get_response_status( resp );
577 | 	if ( status != E_SUCCESS )
578 | 	{
579 | 		dbgPrint( "Command GET GRASPING STATE not successful: %s\n", status_to_str( status ) );
580 | 		free( resp );
581 | 		return 0;
582 | 	}
583 | 
584 | 	free( resp );
585 | 
586 | 	dbgPrint("GRASPING STATUS: %s\n", status_to_str (status) );
587 | 
588 | 	return (int) resp[2];
589 | }
590 | 
591 | 
592 | float getOpeningSpeedForce(unsigned char cmd, int auto_update)
593 | {
594 |     status_t status;
595 |     int res;
596 |     unsigned char payload[3];
597 |     unsigned char *resp;
598 |     unsigned int resp_len;
599 |     std::string names[] = { "opening", "speed", "force", "???" };
600 | 
601 |     // Payload = 0, except for auto update
602 |     memset(payload, 0, 3);
603 |     if (auto_update > 0) {
604 |         payload[0] = 0x01;
605 |         payload[1] = (auto_update & 0xff);
606 |         payload[2] = ((auto_update & 0xff00) >> 8);
607 |     }
608 | 
609 |     // Submit command and wait for response. Expecting exactly 4 bytes response payload.
610 |     res = cmd_submit(cmd, payload, 3, false, &resp, &resp_len ); // 0x43
611 |     if (res != 6) {
612 |         dbgPrint( "Response payload length doesn't match (is %d, expected 3)\n", res );
613 |         if ( res > 0 ) free( resp );
614 |         return 0;
615 |     }
616 | 
617 |     // Check response status
618 |     status = cmd_get_response_status( resp );
619 |     if ( status != E_SUCCESS )	{
620 |         const char *info = names[3].c_str();
621 |         if (cmd >= 0x43 && cmd <= 0x45)
622 |             info = names[cmd-0x43].c_str();
623 |         dbgPrint( "Command 0x%02X get %s not successful: %s\n", cmd, info, status_to_str( status ) );
624 |         free( resp );
625 |         return 0;
626 |     }
627 | 
628 |     float r = convert(&resp[2]);
629 |     free( resp );
630 |     return r;
631 | }
632 | 
633 | /** \brief Read measured opening (width/position) from gripper (0x43).
634 |  *  \param auto_update Request periodic updates (unit: ms) from the gripper; responses need to be read out elsewhere.
635 |  */
636 | float getOpening(int auto_update) {
637 |     return getOpeningSpeedForce(0x43, auto_update);
638 | }
639 | 
640 | /** \brief Read measured speed from gripper (0x44).
641 |  *  \param auto_update Request periodic updates (unit: ms) from the gripper; responses need to be read out elsewhere.
642 |  */
643 | float getSpeed(int auto_update) {
644 |     return getOpeningSpeedForce(0x44, auto_update);
645 | }
646 | 
647 | /** \brief Read measured force from gripper (0x45).
648 |  *  \param auto_update Request periodic updates (unit: ms) from the gripper; responses need to be read out elsewhere.
649 |  */
650 | float getForce(int auto_update){
651 |     return getOpeningSpeedForce(0x45, auto_update);
652 | }
653 | 
654 | 
655 | int getAcceleration( void )  
656 | {
657 | 	status_t status;
658 | 	int res;
659 | 	unsigned char payload[6];
660 | 	unsigned char *resp;
661 | 	unsigned int resp_len;
662 | 	unsigned char vResult[4];
663 | 
664 | 	// Don't use automatic update, so the payload bytes are 0.
665 | 	memset( payload, 0, 1 );
666 | 
667 | 	// Submit command and wait for response. Expecting exactly 4 bytes response payload.
668 | 	res = cmd_submit( 0x31, payload, 0, false, &resp, &resp_len );
669 | 	if ( res != 6 )
670 | 	{
671 | 		dbgPrint( "Response payload length doesn't match (is %d, expected 3)\n", res );
672 | 		if ( res > 0 ) free( resp );
673 | 		return 0;
674 | 	}
675 | 
676 | 	// Check response status
677 | 	status = cmd_get_response_status( resp );
678 | 	if ( status != E_SUCCESS )
679 | 	{
680 | 		dbgPrint( "Command GET ACCELERATION not successful: %s\n", status_to_str( status ) );
681 | 		free( resp );
682 | 		return 0;
683 | 	}
684 | 	
685 | 	vResult[0] = resp[2];
686 | 	vResult[1] = resp[3];
687 | 	vResult[2] = resp[4];
688 | 	vResult[3] = resp[5];
689 | 	
690 | 	free( resp );
691 | 
692 | 	return convert(vResult);
693 | 
694 | 	//return (int) resp[2];
695 | }
696 | 
697 | int getGraspingForceLimit( void )  
698 | {
699 | 	status_t status;
700 | 	int res;
701 | 	unsigned char payload[6];
702 | 	unsigned char *resp;
703 | 	unsigned int resp_len;
704 | 	unsigned char vResult[4];
705 | 
706 | 	// Don't use automatic update, so the payload bytes are 0.
707 | 	memset( payload, 0, 1 );
708 | 
709 | 	// Submit command and wait for response. Expecting exactly 4 bytes response payload.
710 | 	res = cmd_submit( 0x33, payload, 0, false, &resp, &resp_len );
711 | 	if ( res != 6 )
712 | 	{
713 | 		dbgPrint( "Response payload length doesn't match (is %d, expected 3)\n", res );
714 | 		if ( res > 0 ) free( resp );
715 | 		return 0;
716 | 	}
717 | 
718 | 	// Check response status
719 | 	status = cmd_get_response_status( resp );
720 | 	if ( status != E_SUCCESS )
721 | 	{
722 | 		dbgPrint( "Command GET GRASPING FORCE not successful: %s\n", status_to_str( status ) );
723 | 		free( resp );
724 | 		return 0;
725 | 	}
726 | 	
727 | 	vResult[0] = resp[2];
728 | 	vResult[1] = resp[3];
729 | 	vResult[2] = resp[4];
730 | 	vResult[3] = resp[5];
731 | 	
732 | 	free( resp );
733 | 
734 | 	return convert(vResult);
735 | 
736 | 	//return (int) resp[2];
737 | }
738 | 
739 | // MAIN
740 | /*
741 | void test( void )
742 | {
743 | 	while( 1 )
744 | 	{
745 | 		dbgPrint("MOVE\n");
746 | 		move( 0.0f, 60.0f );
747 | 		getOpening();
748 | 
749 | 		//sleep(1);
750 | 		getForce();
751 | 		//sleep(1);
752 | 
753 | 		dbgPrint("HOMING\n");
754 | 		homing();
755 | 		getOpening();
756 | 	}
757 | }
758 | */
759 | 
760 | //------------------------------------------------------------------------
761 | // Functions
762 | //------------------------------------------------------------------------
763 | 


--------------------------------------------------------------------------------
/wsg_50_driver/src/interface.cpp:
--------------------------------------------------------------------------------
  1 | //======================================================================
  2 | /**
  3 |  *  @file
  4 |  *  interface.c
  5 |  *
  6 |  *  @section interface.c_general General file information
  7 |  *
  8 |  *  @brief
  9 |  *  
 10 |  *
 11 |  *  @author wolfer
 12 |  *  @date	07.07.2011
 13 |  *  
 14 |  *  
 15 |  *  @section interface.c_copyright Copyright
 16 |  *  
 17 |  *  Copyright 2011 Weiss Robotics, D-71636 Ludwigsburg, Germany
 18 |  *  
 19 |  *  Redistribution and use in source and binary forms, with or without
 20 |  *  modification, are permitted provided that the following conditions are met:
 21 |  *
 22 |  *     * Redistributions of source code must retain the above copyright
 23 |  *       notice, this list of conditions and the following disclaimer.
 24 |  *     * Redistributions in binary form must reproduce the above copyright
 25 |  *       notice, this list of conditions and the following disclaimer in the
 26 |  *       documentation and/or other materials provided with the distribution.
 27 |  *     * Neither the name of the and Weiss Robotics GmbH nor the names of its 
 28 |  *       contributors may be used to endorse or promote products derived from
 29 |  *	 this software without specific prior written permission.
 30 |  *
 31 |  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 32 |  *  AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 33 |  *  IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 34 |  *  ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
 35 |  *  LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 36 |  *  CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 37 |  *  SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 38 |  *  INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 39 |  *  CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 40 |  *  ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 41 |  *  POSSIBILITY OF SUCH DAMAGE.
 42 |  *
 43 |  */
 44 | //======================================================================
 45 | 
 46 | 
 47 | //------------------------------------------------------------------------
 48 | // Includes
 49 | //------------------------------------------------------------------------
 50 | 
 51 | #include <stdio.h>
 52 | #include <string.h>
 53 | 
 54 | #include "wsg_50/common.h"
 55 | #include "wsg_50/interface.h"
 56 | 
 57 | // Available interfaces
 58 | #include "wsg_50/tcp.h"
 59 | #include "wsg_50/udp.h"
 60 | #include "wsg_50/serial.h"
 61 | 
 62 | 
 63 | //------------------------------------------------------------------------
 64 | // Macros
 65 | //------------------------------------------------------------------------
 66 | 
 67 | 
 68 | //------------------------------------------------------------------------
 69 | // Typedefs, enums, structs
 70 | //------------------------------------------------------------------------
 71 | 
 72 | 
 73 | //------------------------------------------------------------------------
 74 | // Global variables
 75 | //------------------------------------------------------------------------
 76 | 
 77 | // Interface structs
 78 | extern const interface_t tcp;
 79 | extern const interface_t udp;
 80 | extern const interface_t serial;
 81 | 
 82 | // Collection of interfaces, NULL terminated
 83 | static const interface_t *interfaces[] =
 84 | {
 85 | 	&tcp,
 86 | 	&udp,
 87 | 	&serial,
 88 | 	NULL
 89 | };
 90 | 
 91 | 
 92 | //------------------------------------------------------------------------
 93 | // Local function prototypes
 94 | //------------------------------------------------------------------------
 95 | 
 96 | 
 97 | //------------------------------------------------------------------------
 98 | // Unit Testing
 99 | //------------------------------------------------------------------------
100 | 
101 | 
102 | //------------------------------------------------------------------------
103 | // Function implementation
104 | //------------------------------------------------------------------------
105 | 
106 | /**
107 |  * Get interface with the given name
108 |  *
109 |  * @param *name		Interface name string
110 |  *
111 |  * @return Pointer to interface struct
112 |  */
113 | 
114 | const interface_t * interface_get( const char *name )
115 | {
116 | 	unsigned int i = 0;
117 | 
118 | 	while ( interfaces[i] != NULL )
119 | 	{
120 | 		if ( strcmp( name, interfaces[i]->name ) == 0 ) return interfaces[i];
121 | 		i++;
122 | 	}
123 | 
124 | 	return NULL;
125 | }
126 | 
127 | 
128 | //------------------------------------------------------------------------
129 | // Test implementation
130 | //------------------------------------------------------------------------
131 | 


--------------------------------------------------------------------------------
/wsg_50_driver/src/main.cpp:
--------------------------------------------------------------------------------
  1 | /*
  2 |  * WSG 50 ROS NODE
  3 |  * Copyright (c) 2012, Robotnik Automation, SLL
  4 |  * All rights reserved.
  5 |  *
  6 |  * Redistribution and use in source and binary forms, with or without
  7 |  * modification, are permitted provided that the following conditions are met:
  8 |  *
  9 |  *     * Redistributions of source code must retain the above copyright
 10 |  *       notice, this list of conditions and the following disclaimer.
 11 |  *     * Redistributions in binary form must reproduce the above copyright
 12 |  *       notice, this list of conditions and the following disclaimer in the
 13 |  *       documentation and/or other materials provided with the distribution.
 14 |  *     * Neither the name of the Robotnik Automation, SLL. nor the names of its
 15 |  *       contributors may be used to endorse or promote products derived from
 16 |  *       this software without specific prior written permission.
 17 |  *
 18 |  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 19 |  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 20 |  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 21 |  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
 22 |  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 23 |  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 24 |  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 25 |  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 26 |  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 27 |  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 28 |  * POSSIBILITY OF SUCH DAMAGE.
 29 |  *
 30 |  * \author Marc Benetó (mbeneto@robotnik.es)
 31 |  * \brief WSG-50 ROS driver.
 32 |  */
 33 | 
 34 | 
 35 | //------------------------------------------------------------------------
 36 | // Includes
 37 | //------------------------------------------------------------------------
 38 | 
 39 | 
 40 | #include <signal.h>
 41 | #include <stdio.h>
 42 | #include <stdlib.h>
 43 | #include <string.h>
 44 | #include <assert.h>
 45 | #include <thread>
 46 | #include <chrono>
 47 | 
 48 | #include "wsg_50/common.h"
 49 | #include "wsg_50/cmd.h"
 50 | #include "wsg_50/msg.h"
 51 | #include "wsg_50/functions.h"
 52 | 
 53 | #include <ros/ros.h>
 54 | #include "std_msgs/String.h"
 55 | #include "std_srvs/Empty.h"
 56 | #include "wsg_50_common/Status.h"
 57 | #include "wsg_50_common/Move.h"
 58 | #include "wsg_50_common/Conf.h"
 59 | #include "wsg_50_common/Incr.h"
 60 | #include "wsg_50_common/Cmd.h"
 61 | 
 62 | #include "sensor_msgs/JointState.h"
 63 | #include "std_msgs/Float32.h"
 64 | #include "std_msgs/Bool.h"
 65 | 
 66 | 
 67 | //------------------------------------------------------------------------
 68 | // Local macros
 69 | //------------------------------------------------------------------------
 70 | 
 71 | 
 72 | //------------------------------------------------------------------------
 73 | // Typedefs, enums, structs
 74 | //------------------------------------------------------------------------
 75 | 
 76 | #define GRIPPER_MAX_OPEN 110.0
 77 | #define GRIPPER_MIN_OPEN 0.0
 78 | 
 79 | //------------------------------------------------------------------------
 80 | // Global variables
 81 | //------------------------------------------------------------------------
 82 | 
 83 | float increment;
 84 | bool objectGraspped;
 85 | 
 86 | int g_timer_cnt = 0;
 87 | ros::Publisher g_pub_state, g_pub_joint, g_pub_moving;
 88 | bool g_ismoving = false, g_mode_script = false, g_mode_periodic = false, g_mode_polling = false;
 89 | float g_goal_position = NAN, g_goal_speed = NAN, g_speed = 10.0;
 90 | 
 91 | //------------------------------------------------------------------------
 92 | // Unit testing
 93 | //------------------------------------------------------------------------
 94 | 
 95 | 
 96 | //------------------------------------------------------------------------
 97 | // Local function prototypes
 98 | //------------------------------------------------------------------------
 99 | 
100 | 
101 | //------------------------------------------------------------------------
102 | // Function implementation
103 | //------------------------------------------------------------------------
104 | 
105 | 
106 | bool moveSrv(wsg_50_common::Move::Request &req, wsg_50_common::Move::Response &res)
107 | {
108 | 	if ( (req.width >= 0.0 && req.width <= 110.0) && (req.speed > 0.0 && req.speed <= 420.0) ){
109 |   		ROS_INFO("Moving to %f position at %f mm/s.", req.width, req.speed);
110 | 		res.error = move(req.width, req.speed, false);
111 | 	}else if (req.width < 0.0 || req.width > 110.0){
112 | 		ROS_ERROR("Imposible to move to this position. (Width values: [0.0 - 110.0] ");
113 | 		res.error = 255;
114 | 		return false;
115 | 	}else{
116 | 	        ROS_WARN("Speed values are outside the gripper's physical limits ([0.1 - 420.0])  Using clamped values.");
117 | 		res.error = move(req.width, req.speed, false);
118 | 	}
119 | 
120 | 	ROS_INFO("Target position reached.");
121 |   	return true;
122 | }
123 | 
124 | bool graspSrv(wsg_50_common::Move::Request &req, wsg_50_common::Move::Response &res)
125 | {
126 | 	if ( (req.width >= 0.0 && req.width <= 110.0) && (req.speed > 0.0 && req.speed <= 420.0) ){
127 |         ROS_INFO("Grasping object at %f with %f mm/s.", req.width, req.speed);
128 | 		res.error = grasp(req.width, req.speed);
129 | 	}else if (req.width < 0.0 || req.width > 110.0){
130 | 		ROS_ERROR("Imposible to move to this position. (Width values: [0.0 - 110.0] ");
131 | 		res.error = 255;
132 | 		return false;
133 | 	}else{
134 | 	        ROS_WARN("Speed or position values are outside the gripper's physical limits (Position: [0.0 - 110.0] / Speed: [0.1 - 420.0])  Using clamped values.");
135 | 		res.error = grasp(req.width, req.speed);
136 | 	}
137 | 
138 | 	ROS_INFO("Object grasped correctly.");
139 | 	objectGraspped=true;
140 |   	return true;
141 | }
142 | 
143 | bool incrementSrv(wsg_50_common::Incr::Request &req, wsg_50_common::Incr::Response &res)
144 | {
145 | 	if (req.direction == "open"){
146 | 
147 | 		if (!objectGraspped){
148 | 
149 | 			float currentWidth = getOpening();
150 | 			float nextWidth = currentWidth + req.increment;
151 | 			if ( (currentWidth < GRIPPER_MAX_OPEN) && nextWidth < GRIPPER_MAX_OPEN ){
152 | 				//grasp(nextWidth, 1);
153 | 				move(nextWidth,20, true);
154 | 				currentWidth = nextWidth;
155 | 			}else if( nextWidth >= GRIPPER_MAX_OPEN){
156 | 				//grasp(GRIPPER_MAX_OPEN, 1);
157 | 				move(GRIPPER_MAX_OPEN,1, true);
158 | 				currentWidth = GRIPPER_MAX_OPEN;
159 | 			}
160 | 		}else{
161 | 			ROS_INFO("Releasing object...");
162 | 			release(GRIPPER_MAX_OPEN, 20);
163 | 			objectGraspped = false;
164 | 		}
165 | 	}else if (req.direction == "close"){
166 | 
167 | 		if (!objectGraspped){
168 | 
169 | 			float currentWidth = getOpening();
170 | 			float nextWidth = currentWidth - req.increment;
171 | 
172 | 			if ( (currentWidth > GRIPPER_MIN_OPEN) && nextWidth > GRIPPER_MIN_OPEN ){
173 | 				//grasp(nextWidth, 1);
174 | 				move(nextWidth,20, true);
175 | 				currentWidth = nextWidth;
176 | 			}else if( nextWidth <= GRIPPER_MIN_OPEN){
177 | 				//grasp(GRIPPER_MIN_OPEN, 1);
178 | 				move(GRIPPER_MIN_OPEN,1, true);
179 | 				currentWidth = GRIPPER_MIN_OPEN;
180 | 			}
181 | 		}
182 | 	}
183 | 	return true;
184 | }
185 | 
186 | bool releaseSrv(wsg_50_common::Move::Request &req, wsg_50_common::Move::Response &res)
187 | {
188 | 	if ( (req.width >= 0.0 && req.width <= 110.0) && (req.speed > 0.0 && req.speed <= 420.0) ){
189 |   		ROS_INFO("Releasing to %f position at %f mm/s.", req.width, req.speed);
190 | 		res.error = release(req.width, req.speed);
191 | 	}else if (req.width < 0.0 || req.width > 110.0){
192 | 		ROS_ERROR("Imposible to move to this position. (Width values: [0.0 - 110.0] ");
193 | 		res.error = 255;
194 | 		return false;
195 | 	}else{
196 | 	        ROS_WARN("Speed or position values are outside the gripper's physical limits (Position: [0.0 - 110.0] / Speed: [0.1 - 420.0])  Using clamped values.");
197 | 		res.error = release(req.width, req.speed);
198 | 	}
199 | 	ROS_INFO("Object released correctly.");
200 |   	return true;
201 | }
202 | 
203 | bool homingSrv(std_srvs::Empty::Request &req, std_srvs::Empty::Request &res)
204 | {
205 | 	ROS_INFO("Homing...");
206 | 	homing();
207 | 	ROS_INFO("Home position reached.");
208 | 	return true;
209 | }
210 | 
211 | bool stopSrv(std_srvs::Empty::Request &req, std_srvs::Empty::Request &res)
212 | {
213 | 	ROS_WARN("Stop!");
214 | 	stop();
215 | 	ROS_WARN("Stopped.");
216 | 	return true;
217 | }
218 | 
219 | bool setAccSrv(wsg_50_common::Conf::Request &req, wsg_50_common::Conf::Response &res)
220 | {
221 | 	setAcceleration(req.val);
222 | 	return true;
223 | }
224 | 
225 | bool setForceSrv(wsg_50_common::Conf::Request &req, wsg_50_common::Conf::Response &res)
226 | {
227 | 	setGraspingForceLimit(req.val);
228 | 	return true;
229 | }
230 | 
231 | bool ackSrv(std_srvs::Empty::Request &req, std_srvs::Empty::Request &res)
232 | {
233 | 	ack_fault();
234 | 	return true;
235 | }
236 | 
237 | /** \brief Callback for goal_position topic (in appropriate modes) */
238 | void position_cb(const wsg_50_common::Cmd::ConstPtr& msg)
239 | {
240 |     g_speed = msg->speed; g_goal_position = msg->pos;
241 |     // timer_cb() will send command to gripper
242 | 
243 |     if (g_mode_periodic) {
244 |         // Send command to gripper without waiting for a response
245 |         // read_thread() handles responses
246 |         // read/write may be simultaneous, therefore no mutex
247 |         stop(true);
248 |         if (move(g_goal_position, g_speed, false, true) != 0)
249 |             ROS_ERROR("Failed to send MOVE command");
250 |     }
251 | }
252 | 
253 | /** \brief Callback for goal_speed topic (in appropriate modes) */
254 | void speed_cb(const std_msgs::Float32::ConstPtr& msg)
255 | {
256 |     g_goal_speed = msg->data; g_speed = msg->data;
257 |     // timer_cb() will send command to gripper
258 | }
259 | 
260 | /** \brief Loop for state polling in modes script and polling. Also sends command in script mode. */
261 | void timer_cb(const ros::TimerEvent& ev)
262 | {
263 | 	// ==== Get state values by built-in commands ====
264 | 	gripper_response info;
265 | 	float acc = 0.0;
266 | 	info.speed = 0.0;
267 | 
268 |     if (g_mode_polling) {
269 |         const char * state = systemState();
270 |         if (!state)
271 |             return;
272 |         info.state_text = std::string(state);
273 | 		info.position = getOpening();
274 | 		acc = getAcceleration();
275 | 		info.f_motor = getForce();//getGraspingForce();
276 | 
277 |     } else if (g_mode_script) {
278 | 		// ==== Call custom measure-and-move command ====
279 | 		int res = 0;
280 | 		if (!std::isnan(g_goal_position)) {
281 | 			ROS_INFO("Position command: pos=%5.1f, speed=%5.1f", g_goal_position, g_speed);
282 |             res = script_measure_move(1, g_goal_position, g_speed, info);
283 | 		} else if (!std::isnan(g_goal_speed)) {
284 | 			ROS_INFO("Velocity command: speed=%5.1f", g_goal_speed);
285 |             res = script_measure_move(2, 0, g_goal_speed, info);
286 | 		} else
287 |             res = script_measure_move(0, 0, 0, info);
288 | 		if (!std::isnan(g_goal_position))
289 | 			g_goal_position = NAN;
290 | 		if (!std::isnan(g_goal_speed))
291 | 			g_goal_speed = NAN;
292 | 
293 | 		if (!res) {
294 | 			ROS_ERROR("Measure-and-move command failed");
295 | 			return;
296 | 		}
297 | 
298 | 		// ==== Moving msg ====
299 | 		if (g_ismoving != info.ismoving) {
300 | 			std_msgs::Bool moving_msg;
301 | 			moving_msg.data = info.ismoving;
302 | 			g_pub_moving.publish(moving_msg);
303 | 			g_ismoving = info.ismoving;
304 | 		}
305 |     } else
306 |         return;
307 | 
308 | 	// ==== Status msg ====
309 | 	wsg_50_common::Status status_msg;
310 | 	status_msg.status = info.state_text;
311 | 	status_msg.width = info.position;
312 | 	status_msg.speed = info.speed;
313 | 	status_msg.acc = acc;
314 | 	status_msg.force = info.f_motor;
315 | 	status_msg.force_finger0 = info.f_finger0;
316 | 	status_msg.force_finger1 = info.f_finger1;
317 | 
318 | 	g_pub_state.publish(status_msg);
319 | 
320 | 
321 | 	// ==== Joint state msg ====
322 |     // \todo Use name of node for joint names
323 | 	sensor_msgs::JointState joint_states;
324 | 	joint_states.header.stamp = ros::Time::now();;
325 | 	joint_states.header.frame_id = "wsg50_base_link";
326 | 	joint_states.name.push_back("wsg50_finger_left_joint");
327 | 	joint_states.name.push_back("wsg50_finger_right_joint");
328 | 	joint_states.position.resize(2);
329 | 
330 | 	joint_states.position[0] = -info.position/2000.0;
331 | 	joint_states.position[1] = info.position/2000.0;
332 | 	joint_states.velocity.resize(2);
333 |     joint_states.velocity[0] = info.speed/1000.0;
334 |     joint_states.velocity[1] = info.speed/1000.0;
335 | 	joint_states.effort.resize(2);
336 | 	joint_states.effort[0] = info.f_motor;
337 | 	joint_states.effort[1] = info.f_motor;
338 | 
339 | 	g_pub_joint.publish(joint_states);
340 | 
341 | 	// printf("Timer, last duration: %6.1f\n", ev.profile.last_duration.toSec() * 1000.0);
342 | }
343 | 
344 | 
345 | /** \brief Reads gripper responses in auto_update mode. The gripper pushes state messages in regular intervals. */
346 | void read_thread(int interval_ms)
347 | {
348 |     ROS_INFO("Thread started");
349 | 
350 |     status_t status;
351 |     int res;
352 |     bool pub_state = false;
353 | 
354 |     double rate_exp = 1000.0 / (double)interval_ms;
355 |     std::string names[3] = { "opening", "speed", "force" };
356 | 
357 |     // Prepare messages
358 |     wsg_50_common::Status status_msg;
359 |     status_msg.status = "UNKNOWN";
360 | 
361 |     sensor_msgs::JointState joint_states;
362 |     joint_states.header.frame_id = "wsg50_base_link";
363 |     joint_states.name.push_back("wsg50_finger_left_joint");
364 |     joint_states.name.push_back("wsg50_finger_right_joint");
365 |     joint_states.position.resize(2);
366 |     joint_states.velocity.resize(2);
367 |     joint_states.effort.resize(2);
368 | 
369 |     // Request automatic updates (error checking is done below)
370 |     getOpening(interval_ms);
371 |     getSpeed(interval_ms);
372 |     getForce(interval_ms);
373 | 
374 | 
375 |     msg_t msg; msg.id = 0; msg.data = 0; msg.len = 0;
376 |     int cnt[3] = {0,0,0};
377 |     auto time_start = std::chrono::system_clock::now();
378 | 
379 | 
380 |     while (g_mode_periodic) {
381 |         // Receive gripper response
382 |         msg_free(&msg);
383 |         res = msg_receive( &msg );
384 |         if (res < 0 || msg.len < 2) {
385 |             ROS_ERROR("Gripper response failure");
386 |             continue;
387 |         }
388 | 
389 |         float val = 0.0;
390 |         status = cmd_get_response_status(msg.data);
391 | 
392 |         // Decode float for opening/speed/force
393 |         if (msg.id >= 0x43 && msg.id <= 0x45 && msg.len == 6) {
394 |             if (status != E_SUCCESS) {
395 |                 ROS_ERROR("Gripper response failure for opening/speed/force\n");
396 |                 continue;
397 |             }
398 |             val = convert(&msg.data[2]);
399 |         }
400 | 
401 |         // Handle response types
402 |         int motion = -1;
403 |         switch (msg.id) {
404 |         /*** Opening ***/
405 |         case 0x43:
406 |             status_msg.width = val;
407 |             pub_state = true;
408 |             cnt[0]++;
409 |             break;
410 | 
411 |         /*** Speed ***/
412 |         case 0x44:
413 |             status_msg.speed = val;
414 |             cnt[1]++;
415 |             break;
416 | 
417 |         /*** Force ***/
418 |         case 0x45:
419 |             status_msg.force = val;
420 |             cnt[2]++;
421 |             break;
422 | 
423 |         /*** Move ***/
424 |         // Move commands are sent from outside this thread
425 |         case 0x21:
426 |             if (status == E_SUCCESS) {
427 |                 ROS_INFO("Position reached");
428 |                 motion = 0;
429 |             } else if (status == E_AXIS_BLOCKED) {
430 |                 ROS_INFO("Axis blocked");
431 |                 motion = 0;
432 |             } else if (status == E_CMD_PENDING) {
433 |                 ROS_INFO("Movement started");
434 |                 motion = 1;
435 |             } else if (status == E_ALREADY_RUNNING) {
436 |                 ROS_INFO("Movement error: already running");
437 |             } else if (status == E_CMD_ABORTED) {
438 |                 ROS_INFO("Movement aborted");
439 |                 motion = 0;
440 |             } else {
441 |                 ROS_INFO("Movement error");
442 |                 motion = 0;
443 |             }
444 |             break;
445 | 
446 |         /*** Stop ***/
447 |         // Stop commands are sent from outside this thread
448 |         case 0x22:
449 |             // Stop command; nothing to do
450 |             break;
451 |         default:
452 |             ROS_INFO("Received unknown respone 0x%02x (%2dB)\n", msg.id, msg.len);
453 |         }
454 | 
455 |         // ***** PUBLISH motion message
456 |         if (motion == 0 || motion == 1) {
457 |             std_msgs::Bool moving_msg;
458 |             moving_msg.data = motion;
459 |             g_pub_moving.publish(moving_msg);
460 |             g_ismoving = motion;
461 |         }
462 | 
463 |         // ***** PUBLISH state message & joint message
464 |         if (pub_state) {
465 |             pub_state = false;
466 |             g_pub_state.publish(status_msg);
467 | 
468 |             joint_states.header.stamp = ros::Time::now();;
469 |             joint_states.position[0] = -status_msg.width/2000.0;
470 |             joint_states.position[1] = status_msg.width/2000.0;
471 |             joint_states.velocity[0] = status_msg.speed/1000.0;
472 |             joint_states.velocity[1] = status_msg.speed/1000.0;
473 |             joint_states.effort[0] = status_msg.force;
474 |             joint_states.effort[1] = status_msg.force;
475 |             g_pub_joint.publish(joint_states);
476 |         }
477 | 
478 |         // Check # of received messages regularly
479 |         std::chrono::duration<float> t = std::chrono::system_clock::now() - time_start;
480 |         double t_ = t.count();
481 |         if (t_ > 5.0) {
482 |             time_start = std::chrono::system_clock::now();
483 |             //printf("Infos for %5.1fHz, %5.1fHz, %5.1fHz\n", (double)cnt[0]/t_, (double)cnt[1]/t_, (double)cnt[2]/t_);
484 | 
485 |             std::string info = "Rates for ";
486 |             for (int i=0; i<3; i++) {
487 |                 double rate_is = (double)cnt[i]/t_;
488 |                 info += names[i] + ": " + std::to_string((int)rate_is) + "Hz, ";
489 |                 if (rate_is == 0.0)
490 |                     ROS_ERROR("Did not receive data for %s", names[i].c_str());
491 |             }
492 |             ROS_DEBUG_STREAM((info + " expected: " + std::to_string((int)rate_exp) + "Hz").c_str());
493 |             cnt[0] = 0; cnt[1] = 0; cnt[2] = 0;
494 |         }
495 | 
496 | 
497 |     }
498 | 
499 |     // Disable automatic updates
500 |     // TODO: The functions will receive an unexpected response
501 |     getOpening(0);
502 |     getSpeed(0);
503 |     getForce(0);
504 | 
505 |     ROS_INFO("Thread ended");
506 | }
507 | 
508 | void sigint_handler(int sig) {
509 |     ROS_INFO("Exiting...");
510 |     g_mode_periodic = false;
511 |     g_mode_script = false;
512 |     g_mode_polling = false;
513 |     ros::shutdown();
514 | }
515 | 
516 | /**
517 |  * The main function
518 |  */
519 | 
520 | int main( int argc, char **argv )
521 | {
522 |    ros::init(argc, argv, "wsg_50");
523 |    ros::NodeHandle nh("~");
524 |    signal(SIGINT, sigint_handler);
525 | 
526 |    std::string ip, protocol, com_mode;
527 |    int port, local_port;
528 |    double rate, grasping_force;
529 |    bool use_udp = false;
530 | 
531 |    nh.param("ip", ip, std::string("192.168.1.20"));
532 |    nh.param("port", port, 1000);
533 |    nh.param("local_port", local_port, 1501);
534 |    nh.param("protocol", protocol, std::string(""));
535 |    nh.param("com_mode", com_mode, std::string(""));
536 |    nh.param("rate", rate, 1.0); // With custom script, up to 30Hz are possible
537 |    nh.param("grasping_force", grasping_force, 0.0);
538 | 
539 |    if (protocol == "udp")
540 |        use_udp = true;
541 |    else
542 |        protocol = "tcp";
543 |    if (com_mode == "script")
544 |        g_mode_script = true;
545 |    else if (com_mode == "auto_update")
546 |        g_mode_periodic = true;
547 |    else {
548 |        com_mode = "polling";
549 |        g_mode_polling = true;
550 |    }
551 | 
552 |    ROS_INFO("Connecting to %s:%d (%s); communication mode: %s ...", ip.c_str(), port, protocol.c_str(), com_mode.c_str());
553 | 
554 |    // Connect to device using TCP/USP
555 |    int res_con;
556 |    if (!use_udp)
557 |        res_con = cmd_connect_tcp( ip.c_str(), port );
558 |    else
559 |        res_con = cmd_connect_udp(local_port, ip.c_str(), port );
560 | 
561 |    if (res_con == 0 ) {
562 |         ROS_INFO("Gripper connection stablished");
563 | 
564 | 		// Services
565 |         ros::ServiceServer moveSS, graspSS, releaseSS, homingSS, stopSS, ackSS, incrementSS, setAccSS, setForceSS;
566 | 
567 |         if (g_mode_script || g_mode_polling) {
568 |             moveSS = nh.advertiseService("move", moveSrv);
569 |             graspSS = nh.advertiseService("grasp", graspSrv);
570 |             releaseSS = nh.advertiseService("release", releaseSrv);
571 |             homingSS = nh.advertiseService("homing", homingSrv);
572 |             stopSS = nh.advertiseService("stop", stopSrv);
573 |             ackSS = nh.advertiseService("ack", ackSrv);
574 |             incrementSS = nh.advertiseService("move_incrementally", incrementSrv);
575 | 
576 |             setAccSS = nh.advertiseService("set_acceleration", setAccSrv);
577 |             setForceSS = nh.advertiseService("set_force", setForceSrv);
578 |         }
579 | 
580 | 		// Subscriber
581 |         ros::Subscriber sub_position, sub_speed;
582 |         if (g_mode_script || g_mode_periodic)
583 |             sub_position = nh.subscribe("goal_position", 5, position_cb);
584 |         if (g_mode_script)
585 |             sub_speed = nh.subscribe("goal_speed", 5, speed_cb);
586 | 
587 | 		// Publisher
588 | 		g_pub_state = nh.advertise<wsg_50_common::Status>("status", 1000);
589 | 		g_pub_joint = nh.advertise<sensor_msgs::JointState>("/joint_states", 10);
590 |         if (g_mode_script || g_mode_periodic)
591 |             g_pub_moving = nh.advertise<std_msgs::Bool>("moving", 10);
592 | 
593 | 		ROS_INFO("Ready to use. Homing and taring now...");
594 | 		homing();
595 |         ros::Duration(0.5).sleep();
596 |         doTare();
597 | 
598 | 		if (grasping_force > 0.0) {
599 | 			ROS_INFO("Setting grasping force limit to %5.1f", grasping_force);
600 | 			setGraspingForceLimit(grasping_force);
601 | 		}
602 | 
603 |         ROS_INFO("Init done. Starting timer/thread with target rate %.1f.", rate);
604 |         std::thread th;
605 |         ros::Timer tmr;
606 |         if (g_mode_polling || g_mode_script)
607 |             tmr = nh.createTimer(ros::Duration(1.0/rate), timer_cb);
608 |         if (g_mode_periodic)
609 |              th = std::thread(read_thread, (int)(1000.0/rate));
610 | 
611 |         ros::spin();
612 | 
613 | 	} else {
614 |         ROS_ERROR("Unable to connect, please check the port and address used.");
615 | 	}
616 | 
617 |    ROS_INFO("Exiting...");
618 |    g_mode_periodic = false;
619 |    g_mode_script = false;
620 |    g_mode_polling = false;
621 |    sleep(1);
622 |    cmd_disconnect();
623 | 
624 | 	return 0;
625 | 
626 | }
627 | 
628 | 
629 | //------------------------------------------------------------------------
630 | // Testing functions
631 | //------------------------------------------------------------------------
632 | 


--------------------------------------------------------------------------------
/wsg_50_driver/src/main_can.cpp:
--------------------------------------------------------------------------------
  1 | /*
  2 |  * WSG 50 ROS NODE
  3 |  * Copyright (c) 2012, Robotnik Automation, SLL
  4 |  * All rights reserved.
  5 |  *
  6 |  * Redistribution and use in source and binary forms, with or without
  7 |  * modification, are permitted provided that the following conditions are met:
  8 |  *
  9 |  *     * Redistributions of source code must retain the above copyright
 10 |  *       notice, this list of conditions and the following disclaimer.
 11 |  *     * Redistributions in binary form must reproduce the above copyright
 12 |  *       notice, this list of conditions and the following disclaimer in the
 13 |  *       documentation and/or other materials provided with the distribution.
 14 |  *     * Neither the name of the Robotnik Automation, SLL. nor the names of its
 15 |  *       contributors may be used to endorse or promote products derived from
 16 |  *       this software without specific prior written permission.
 17 |  *
 18 |  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 19 |  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 20 |  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 21 |  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
 22 |  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 23 |  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 24 |  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 25 |  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 26 |  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 27 |  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 28 |  * POSSIBILITY OF SUCH DAMAGE.
 29 |  *
 30 |  * \author Marc Benetó (mbeneto@robotnik.es)
 31 |  * \brief WSG-50 ROS driver.
 32 |  */
 33 | 
 34 | 
 35 | //------------------------------------------------------------------------
 36 | // Includes
 37 | //------------------------------------------------------------------------
 38 | 
 39 | /// Generic
 40 | #include <stdio.h>
 41 | #include <stdlib.h>
 42 | #include <string.h>
 43 | #include <assert.h>
 44 | #include <fcntl.h> 
 45 | 
 46 | #include "wsg_50/common.h"
 47 | #include "wsg_50/functions_can.h"
 48 | //#include "wsg_50/cmd.h"
 49 | 
 50 | /// ROS
 51 | #include <ros/ros.h>
 52 | #include "std_msgs/String.h"
 53 | #include "wsg_50_common/Status.h"
 54 | #include "wsg_50_common/Move.h"
 55 | #include "std_srvs/Empty.h"
 56 | #include "wsg_50_common/Conf.h"
 57 | #include "wsg_50_common/Incr.h"
 58 | 
 59 | //------------------------------------------------------------------------
 60 | // Local macros
 61 | //------------------------------------------------------------------------
 62 | 
 63 | 
 64 | //------------------------------------------------------------------------
 65 | // Typedefs, enums, structs
 66 | //------------------------------------------------------------------------
 67 | 
 68 | #define GRIPPER_MAX_OPEN 110.0
 69 | #define GRIPPER_MIN_OPEN 0.0
 70 | 
 71 | //------------------------------------------------------------------------
 72 | // Global variables
 73 | //------------------------------------------------------------------------
 74 | 
 75 | float increment;
 76 | bool objectGraspped;
 77 |    
 78 | //------------------------------------------------------------------------
 79 | // Unit testing
 80 | //------------------------------------------------------------------------
 81 | 
 82 | 
 83 | //------------------------------------------------------------------------
 84 | // Local function prototypes
 85 | //------------------------------------------------------------------------
 86 | 
 87 | 
 88 | //------------------------------------------------------------------------
 89 | // Function implementation
 90 | //------------------------------------------------------------------------
 91 | 
 92 | 
 93 | bool moveSrv(wsg_50_common::Move::Request &req, wsg_50_common::Move::Response &res)
 94 | {
 95 | 	if ( (req.width >= 0.0 && req.width <= 110.0) && (req.speed > 0.0 && req.speed <= 420.0) ){
 96 |   		ROS_INFO("Moving to %f position at %f mm/s.", req.width, req.speed);
 97 | 		res.error = move(req.width, req.speed);
 98 | 	}else if (req.width < 0.0 || req.width > 110.0){
 99 | 		ROS_ERROR("Imposible to move to this position. (Width values: [0.0 - 110.0] ");
100 | 		res.error = 255;
101 | 		return false;
102 | 	}else{
103 | 	    ROS_WARN("Speed values are outside the gripper's physical limits ([0.1 - 420.0])  Using clamped values.");
104 | 		res.error = move(req.width, req.speed);
105 | 	}
106 | 
107 | 	ROS_INFO("Target position reached.");
108 |   	return true;
109 | }
110 | 
111 | bool graspSrv(wsg_50_common::Move::Request &req, wsg_50_common::Move::Response &res)
112 | {
113 | 	if ( (req.width >= 0.0 && req.width <= 110.0) && (req.speed > 0.0 && req.speed <= 420.0) ){
114 |   		ROS_INFO("Grasping object at %f mm/s.", req.width, req.speed);
115 | 		res.error = grasp(req.width, req.speed);
116 | 	}else if (req.width < 0.0 || req.width > 110.0){
117 | 		ROS_ERROR("Imposible to move to this position. (Width values: [0.0 - 110.0] ");
118 | 		res.error = 255;
119 | 		return false;
120 | 	}else{
121 | 	        ROS_WARN("Speed or position values are outside the gripper's physical limits (Position: [0.0 - 110.0] / Speed: [0.1 - 420.0])  Using clamped values.");
122 | 		res.error = grasp(req.width, req.speed);
123 | 	}
124 | 
125 | 	ROS_INFO("Object grasped correctly.");
126 | 	objectGraspped=true;
127 |   	return true;
128 | }
129 | 
130 | bool incrementSrv(wsg_50_common::Incr::Request &req, wsg_50_common::Incr::Response &res)
131 | {
132 | 	if (req.direction == "open"){
133 | 	
134 | 		if (!objectGraspped){
135 | 		
136 | 			float currentWidth = getOpening();
137 | 			float nextWidth = currentWidth + req.increment;
138 | 			if ( (currentWidth < GRIPPER_MAX_OPEN) && nextWidth < GRIPPER_MAX_OPEN ){
139 | 				//grasp(nextWidth, 1);
140 | 				move(nextWidth,20);
141 | 				currentWidth = nextWidth;
142 | 			}else if( nextWidth >= GRIPPER_MAX_OPEN){
143 | 				//grasp(GRIPPER_MAX_OPEN, 1);
144 | 				move(GRIPPER_MAX_OPEN,1);
145 | 				currentWidth = GRIPPER_MAX_OPEN;
146 | 			}
147 | 		}else{
148 | 			ROS_INFO("Releasing object...");
149 | 			release(GRIPPER_MAX_OPEN, 20);
150 | 			objectGraspped = false;
151 | 		}
152 | 	}else if (req.direction == "close"){
153 | 	
154 | 		if (!objectGraspped){
155 | 
156 | 			float currentWidth = getOpening();
157 | 			float nextWidth = currentWidth - req.increment;
158 | 		
159 | 			if ( (currentWidth > GRIPPER_MIN_OPEN) && nextWidth > GRIPPER_MIN_OPEN ){
160 | 				//grasp(nextWidth, 1);
161 | 				move(nextWidth,20);
162 | 				currentWidth = nextWidth;
163 | 			}else if( nextWidth <= GRIPPER_MIN_OPEN){
164 | 				//grasp(GRIPPER_MIN_OPEN, 1);
165 | 				move(GRIPPER_MIN_OPEN,1);
166 | 				currentWidth = GRIPPER_MIN_OPEN;
167 | 			}
168 | 		}
169 | 	}
170 | }
171 | 
172 | bool releaseSrv(wsg_50_common::Move::Request &req, wsg_50_common::Move::Response &res)
173 | {
174 | 	if ( (req.width >= 0.0 && req.width <= 110.0) && (req.speed > 0.0 && req.speed <= 420.0) ){
175 |   		ROS_INFO("Releasing to %f position at %f mm/s.", req.width, req.speed);
176 | 		res.error = release(req.width, req.speed);
177 | 	}else if (req.width < 0.0 || req.width > 110.0){
178 | 		ROS_ERROR("Imposible to move to this position. (Width values: [0.0 - 110.0] ");
179 | 		res.error = 255;
180 | 		return false;
181 | 	}else{
182 | 	        ROS_WARN("Speed or position values are outside the gripper's physical limits (Position: [0.0 - 110.0] / Speed: [0.1 - 420.0])  Using clamped values.");
183 | 		res.error = release(req.width, req.speed);
184 | 	}
185 | 	ROS_INFO("Object released correctly.");
186 |   	return true;
187 | }
188 | 
189 | bool homingSrv(std_srvs::Empty::Request &req, std_srvs::Empty::Request &res)
190 | {
191 | 	ROS_INFO("Homing...");
192 | 	homing();
193 | 	ROS_INFO("Home position reached.");
194 | 	return true;
195 | }
196 | /*
197 | bool stopSrv(std_srvs::Empty::Request &req, std_srvs::Empty::Request &res)
198 | {
199 | 	ROS_WARN("Stop!");
200 | 	stop();
201 | 	ROS_WARN("Stopped.");
202 | 	return true;
203 | }
204 | */
205 | bool setAccSrv(wsg_50_common::Conf::Request &req, wsg_50_common::Conf::Response &res)
206 | {
207 | 	setAcceleration(req.val);
208 | 	return true;
209 | }
210 | 
211 | bool setForceSrv(wsg_50_common::Conf::Request &req, wsg_50_common::Conf::Response &res)
212 | {
213 | 	setGraspingForceLimit(req.val);
214 | 	return true;
215 | }
216 | /*
217 | bool ackSrv(std_srvs::Empty::Request &req, std_srvs::Empty::Request &res)
218 | {
219 | 	ack_fault();
220 | 	return true;
221 | }
222 | */
223 | 
224 | 
225 | /**
226 |  * The main function
227 |  */
228 | 
229 | int main( int argc, char **argv )
230 | {
231 |    ros::init(argc, argv, "wsg_50_can");
232 | 
233 |    ros::NodeHandle nh("~");
234 |    std::string device_;
235 |    nh.param("device", device_, device_);
236 |    
237 |    ROS_INFO("WSG 50 - CAN ROS NODE");
238 | 
239 |    // Connect to device using CAN
240 |    if( CAN_connect( device_.c_str() ) )
241 |    {
242 | 
243 | 	// Services
244 | 	
245 |   	ros::ServiceServer moveSS = nh.advertiseService("move", moveSrv);
246 |   	ros::ServiceServer graspSS = nh.advertiseService("grasp", graspSrv);
247 |   	ros::ServiceServer releaseSS = nh.advertiseService("release", releaseSrv); 
248 |   	ros::ServiceServer homingSS = nh.advertiseService("homing", homingSrv);
249 |   	
250 |   	//ros::ServiceServer stopSS = nh.advertiseService("stop", stopSrv);
251 |   	//ros::ServiceServer ackSS = nh.advertiseService("ack", ackSrv);
252 | 	
253 | 	ros::ServiceServer incrementSS = nh.advertiseService("move_incrementally", incrementSrv);
254 | 
255 | 	ros::ServiceServer setAccSS = nh.advertiseService("set_acceleration", setAccSrv);
256 | 	ros::ServiceServer setForceSS = nh.advertiseService("set_force", setForceSrv);
257 | 	
258 | 	// Publisher
259 |   	ros::Publisher state_pub = nh.advertise<wsg_50_common::Status>("status", 1000);
260 | 
261 | 	ROS_INFO("Ready to use.");
262 | 
263 | 	homing();
264 | 
265 | 	ros::Rate loop_rate(10); // loop at 10Hz
266 | 
267 | 	while( ros::ok() ){
268 | 		
269 | 		//Loop waiting for orders and updating the state
270 | 		
271 | 		//Create the msg to send
272 | 		wsg_50_common::Status status_msg;		
273 | 
274 | 		//Get state values
275 | 		//const char * aux;
276 | 		//aux = systemState();
277 | 		int op = getOpening();
278 | 		///int acc = getAcceleration();
279 | 		///int force = getGraspingForceLimit();
280 | 
281 |     	//std::stringstream ss;
282 | 		
283 | 		//ss << aux;
284 | 
285 | 		//status_msg.status = ss.str();
286 | 		status_msg.width = op;
287 | 		///status_msg.acc = acc;
288 | 		///status_msg.force = force;
289 | 
290 | 		state_pub.publish(status_msg);
291 | 		
292 | 		loop_rate.sleep();
293 | 		ros::spinOnce();
294 | 		
295 | 	}
296 | 	
297 | 
298 |    }else{
299 | 
300 | 		ROS_ERROR("Unable to connect via CAN, please check the velocity and node gripper configuration.");
301 | 	
302 |    }
303 | 
304 |    // Disconnect - won't be executed atm. as the endless loop in test()
305 |    // will never return.
306 |   	CAN_disconnect();
307 | 
308 |    return 0;
309 | 
310 | }
311 | 
312 | 
313 | //------------------------------------------------------------------------
314 | // Testing functions
315 | //------------------------------------------------------------------------
316 | 


--------------------------------------------------------------------------------
/wsg_50_driver/src/msg.c:
--------------------------------------------------------------------------------
  1 | //======================================================================
  2 | /**
  3 |  *  @file
  4 |  *  msg.c
  5 |  *
  6 |  *  @section msg.c_general General file information
  7 |  *
  8 |  *  @brief
  9 |  *  Raw send and receive functions for command messages
 10 |  *
 11 |  *  @author wolfer
 12 |  *  @date	07.07.2011
 13 |  *  
 14 |  *  
 15 |  *  @section msg.c_copyright Copyright
 16 |  *  
 17 |  *  Copyright 2011 Weiss Robotics, D-71636 Ludwigsburg, Germany
 18 |  *  
 19 |  *  Redistribution and use in source and binary forms, with or without
 20 |  *  modification, are permitted provided that the following conditions are met:
 21 |  *
 22 |  *     * Redistributions of source code must retain the above copyright
 23 |  *       notice, this list of conditions and the following disclaimer.
 24 |  *     * Redistributions in binary form must reproduce the above copyright
 25 |  *       notice, this list of conditions and the following disclaimer in the
 26 |  *       documentation and/or other materials provided with the distribution.
 27 |  *     * Neither the name of the and Weiss Robotics GmbH nor the names of its 
 28 |  *       contributors may be used to endorse or promote products derived from
 29 |  *	 this software without specific prior written permission.
 30 |  *
 31 |  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 32 |  *  AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 33 |  *  IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 34 |  *  ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
 35 |  *  LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 36 |  *  CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 37 |  *  SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 38 |  *  INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 39 |  *  CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 40 |  *  ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 41 |  *  POSSIBILITY OF SUCH DAMAGE.
 42 |  *
 43 |  */
 44 | //======================================================================
 45 | 
 46 | 
 47 | //------------------------------------------------------------------------
 48 | // Includes
 49 | //------------------------------------------------------------------------
 50 | 
 51 | #include <stdio.h>
 52 | #include <stdlib.h>
 53 | #include <string.h>
 54 | 
 55 | #include "wsg_50/common.h"
 56 | #include "wsg_50/checksum.h"
 57 | #include "wsg_50/interface.h"
 58 | #include "wsg_50/msg.h"
 59 | 
 60 | 
 61 | //------------------------------------------------------------------------
 62 | // Macros
 63 | //------------------------------------------------------------------------
 64 | 
 65 | 
 66 | //------------------------------------------------------------------------
 67 | // Typedefs, enums, structs
 68 | //------------------------------------------------------------------------
 69 | 
 70 | 
 71 | //------------------------------------------------------------------------
 72 | // Global variables
 73 | //------------------------------------------------------------------------
 74 | 
 75 | static const interface_t *interface;
 76 | 
 77 | 
 78 | //------------------------------------------------------------------------
 79 | // Local function prototypes
 80 | //------------------------------------------------------------------------
 81 | 
 82 | 
 83 | //------------------------------------------------------------------------
 84 | // Unit Testing
 85 | //------------------------------------------------------------------------
 86 | 
 87 | 
 88 | //------------------------------------------------------------------------
 89 | // Function implementation
 90 | //------------------------------------------------------------------------
 91 | 
 92 | /**
 93 |  * Receive answer
 94 |  *
 95 |  * @param **response		Data buffer
 96 |  * @param len				Expected size of message
 97 |  *
 98 |  * @return Overall number of bytes received, including header and checksum. -1 on error.
 99 |  */
100 | 
101 | int msg_receive( msg_t *msg )
102 | {
103 | 	int res;
104 | 	unsigned char header[3];			// 1 byte command, 2 bytes payload length
105 | 	unsigned short checksum = 0x50f5;	// Checksum over preamble (0xaa 0xaa 0xaa)
106 | 	unsigned int sync;
107 | 
108 | 	// Syncing - necessary for compatibility with serial interface
109 | 	sync = 0;
110 | 	while( sync != MSG_PREAMBLE_LEN )
111 | 	{
112 | 		res = interface->read( header, 1 );
113 | 		if ( header[0] == MSG_PREAMBLE_BYTE ) sync++;
114 | 	}
115 | 
116 | 	// Read header
117 | 	res = interface->read( header, 3 );
118 | 	if ( res < 3 )
119 | 	{
120 | 		fprintf( stderr, "Failed to receive header data (%d bytes read)\n", res );
121 | 		return -1;
122 | 	}
123 | 
124 | 	// Calculate checksum over header
125 | 	checksum = checksum_update_crc16( header, 3, checksum );
126 | 
127 | 	// Get message id of received
128 | 	msg->id = header[0];
129 | 
130 | 	// Get payload size of received message
131 | 	msg->len = make_short( header[1], header[2] );
132 | 
133 | 	// Allocate space for payload and checksum
134 | 	msg->data = malloc( msg->len + 2u );
135 | 	if ( !msg->data ) return -1;
136 | 
137 | 	// Read payload and checksum
138 | 	res = interface->read( msg->data, msg->len + 2 );
139 | 	if ( res < (int) (msg->len + 2) )
140 | 	{
141 | 		fprintf( stderr, "Not enough data (%d, expected %d)\n", res, msg->len + 2 );
142 | 		return -1;
143 | 	}
144 | 
145 | 	// Check checksum
146 | 	checksum = checksum_update_crc16( msg->data, msg->len + 2, checksum );
147 | 	if ( checksum != 0 )
148 | 	{
149 | 		fprintf( stderr, "Checksum error\n" );
150 | 		return -1;
151 | 	}
152 | 
153 | 	return msg->len + 8;
154 | }
155 | 
156 | 
157 | /**
158 |  * Send command
159 |  *
160 |  * @param id		Command ID
161 |  * @param len		Payload length
162 |  * @param *payload	Payload data
163 |  *
164 |  * @return 0 on success, else -1
165 |  */
166 | 
167 | int msg_send( msg_t *msg )
168 | {
169 | 	unsigned char header[MSG_PREAMBLE_LEN + 3];
170 |     //unsigned char checksum[2];
171 | 	unsigned short crc;
172 | 	int i, res;
173 | 
174 | 	// Preamble
175 | 	for ( i = 0; i < MSG_PREAMBLE_LEN; i++ ) header[i] = MSG_PREAMBLE_BYTE;
176 | 
177 | 	// Command ID
178 | 	header[MSG_PREAMBLE_LEN] = msg->id;
179 | 
180 | 	// Length
181 | 	header[MSG_PREAMBLE_LEN + 1] = lo( msg->len );
182 | 	header[MSG_PREAMBLE_LEN + 2] = hi( msg->len );
183 | 
184 | 	// Checksum
185 | 	crc = checksum_crc16( header, 6 );
186 | 	crc = checksum_update_crc16( msg->data, msg->len, crc );
187 | 
188 |     //checksum[0] = lo( crc );
189 |     //checksum[1] = hi( crc );
190 | 
191 | 	if ( interface->write )
192 | 	{
193 | 
194 | 		unsigned char *buf = malloc( 6 + msg->len + 2 ); // 6+2 fixes (PREAMBLE, ID, PAILOAD / ... / CRC)
195 | 		memcpy( buf, header, 6 );
196 | 		memcpy( buf + 6, msg->data, msg->len );
197 | 		memcpy( buf + 6 + msg->len, (unsigned char *) &crc, 2 );
198 | 
199 | 		res = interface->write( buf, 6 + msg->len + 2 );
200 |         if ( res < 6 + (int)msg->len + 2 )
201 | 		{
202 | 			interface->close();
203 | 			quit( "Failed to submit message checksum" );
204 | 		}
205 | 
206 | 		free( buf );
207 | 
208 | // The following implementation doesn't work:
209 | //
210 | //		// Submit header
211 | //		res = interface->write( header, 6 );
212 | //		if ( res < 6 )
213 | //		{
214 | //			interface->close();
215 | //			quit( "Failed to submit message header" );
216 | //		}
217 | //
218 | //		// Submit payload
219 | //		res = interface->write( msg->data, msg->len );
220 | //		if ( res < msg->len )
221 | //		{
222 | //			interface->close();
223 | //			quit( "Failed to submit message payload" );
224 | //		}
225 | //
226 | //		res = interface->write( (unsigned char *) &crc, 2 );
227 | //		if ( res < sizeof( crc ) )
228 | //		{
229 | //			interface->close();
230 | //			quit( "Failed to submit message checksum" );
231 | //		}
232 | 
233 | //		unsigned short check = checksum_crc16( header, 6 );
234 | //		check = checksum_update_crc16( msg->data, msg->len, check );
235 | //		check = checksum_update_crc16( (unsigned char *) &crc, sizeof( short ), check );
236 | 
237 | 		return msg->len + 8;
238 | 	}
239 | 
240 | 	return -1;
241 | }
242 | 
243 | 
244 | /**
245 |  * Change command interface
246 |  *
247 |  * @param *iface		Pointer to interface struct
248 |  * 						describing new interface
249 |  *
250 |  * @return 0 on success, else -1
251 |  */
252 | 
253 | int msg_change_interface( const interface_t *iface )
254 | {
255 | 	if ( !iface ) return -1;
256 | 
257 | 	if ( interface && iface != interface && interface->close ) interface->close();
258 | 
259 | 	interface = iface;
260 | 
261 | 	return 0;
262 | }
263 | 
264 | 
265 | /**
266 |  * Open command interface
267 |  *
268 |  * @param *iface
269 |  * @param *params		Pointer referencing a struct that holds
270 |  * 						parameters for the interface (e.g. address)
271 |  *
272 |  * @return 0 on success, else -1
273 |  */
274 | 
275 | int msg_open( const interface_t *iface, const void *params )
276 | {
277 | 	int res;
278 | 
279 | 	res = msg_change_interface( iface );
280 | 	if ( res < 0 ) return( res );
281 | 
282 | 	if ( iface->open ) return iface->open( params );
283 | 	else return -1;
284 | }
285 | 
286 | 
287 | /**
288 |  * Close command interface
289 |  */
290 | 
291 | void msg_close( void )
292 | {
293 | 	if ( !interface || !interface->close ) return;
294 | 	interface->close();
295 | }
296 | 
297 | 
298 | /**
299 |  * Free message struct
300 |  *
301 |  * @param *msg		Pointer to message struct
302 |  */
303 | 
304 | void msg_free( msg_t *msg )
305 | {
306 | 	if ( msg->data ) free( msg->data );
307 | 	memset( msg, 0, sizeof( msg ) );
308 | }
309 | 
310 | 
311 | //------------------------------------------------------------------------
312 | // Test implementation
313 | //------------------------------------------------------------------------
314 | 


--------------------------------------------------------------------------------
/wsg_50_driver/src/serial.c:
--------------------------------------------------------------------------------
  1 | //======================================================================
  2 | /**
  3 |  *  @file
  4 |  *  serial.c
  5 |  *
  6 |  *  @section serial.c_general General file information
  7 |  *
  8 |  *  @brief
  9 |  *  
 10 |  *
 11 |  *  @author wolfer
 12 |  *  @date	08.07.2011
 13 |  *  
 14 |  *  
 15 |  *  @section serial.c_copyright Copyright
 16 |  *  
 17 |  *  Copyright 2011 Weiss Robotics, D-71636 Ludwigsburg, Germany
 18 |  *  
 19 |  *  Redistribution and use in source and binary forms, with or without
 20 |  *  modification, are permitted provided that the following conditions are met:
 21 |  *
 22 |  *     * Redistributions of source code must retain the above copyright
 23 |  *       notice, this list of conditions and the following disclaimer.
 24 |  *     * Redistributions in binary form must reproduce the above copyright
 25 |  *       notice, this list of conditions and the following disclaimer in the
 26 |  *       documentation and/or other materials provided with the distribution.
 27 |  *     * Neither the name of the and Weiss Robotics GmbH nor the names of its 
 28 |  *       contributors may be used to endorse or promote products derived from
 29 |  *	 this software without specific prior written permission.
 30 |  *
 31 |  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 32 |  *  AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 33 |  *  IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 34 |  *  ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
 35 |  *  LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 36 |  *  CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 37 |  *  SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 38 |  *  INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 39 |  *  CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 40 |  *  ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 41 |  *  POSSIBILITY OF SUCH DAMAGE.
 42 |  *
 43 |  */
 44 | //======================================================================
 45 | 
 46 | 
 47 | //------------------------------------------------------------------------
 48 | // Includes
 49 | //------------------------------------------------------------------------
 50 | 
 51 | #include <stdio.h>
 52 | #include <stdlib.h>
 53 | #include <string.h>
 54 | 
 55 | #ifdef WIN32
 56 | 	// Note: Windows implementation is different
 57 | 	#include "windows.h"
 58 | #else
 59 | 	#include <fcntl.h>
 60 | 	#include <termios.h>
 61 | 	#include <errno.h>
 62 | 	#include <unistd.h>
 63 | 	#include <sys/types.h>
 64 | 	#include <sys/stat.h>
 65 | #endif
 66 | 
 67 | #include "wsg_50/interface.h"
 68 | #include "wsg_50/serial.h"
 69 | 
 70 | 
 71 | //------------------------------------------------------------------------
 72 | // Macros
 73 | //------------------------------------------------------------------------
 74 | 
 75 | 
 76 | //------------------------------------------------------------------------
 77 | // Typedefs, enums, structs
 78 | //------------------------------------------------------------------------
 79 | 
 80 | 
 81 | //------------------------------------------------------------------------
 82 | // Global variables
 83 | //------------------------------------------------------------------------
 84 | 
 85 | const interface_t serial =
 86 | {
 87 | 	.name = "serial",
 88 | 	.open = &serial_open,
 89 | 	.close = &serial_close,
 90 | 	.read = &serial_read,
 91 | 	.write = &serial_write
 92 | };
 93 | 
 94 | #ifdef WIN32
 95 | 
 96 | #else
 97 | 
 98 | static ser_conn_t conn;
 99 | 
100 | #endif
101 | 
102 | 
103 | //------------------------------------------------------------------------
104 | // Local function prototypes
105 | //------------------------------------------------------------------------
106 | 
107 | static inline tcflag_t __bitrate_to_flag( unsigned int bitrate );
108 | 
109 | 
110 | //------------------------------------------------------------------------
111 | // Unit Testing
112 | //------------------------------------------------------------------------
113 | 
114 | 
115 | //------------------------------------------------------------------------
116 | // Function implementation
117 | //------------------------------------------------------------------------
118 | 
119 | /**
120 |  * Convert integer bitrate to flag
121 |  *
122 |  * @param bitrate		Bitrate
123 |  *
124 |  * @return Bitrate flag for serial driver
125 |  */
126 | 
127 | static inline tcflag_t __bitrate_to_flag( unsigned int bitrate )
128 | {
129 | 	switch( bitrate )
130 | 	{
131 | 		case   1200: return   B1200;
132 | 		case   2400: return   B2400;
133 | 		case   4800: return   B4800;
134 | 		case   9600: return   B9600;
135 | 		case  19200: return  B19200;
136 | 		case  38400: return  B38400;
137 | 		case  57600: return  B57600;
138 | 		case 115200: return B115200;
139 | 		case 230400: return B230400;
140 | 		case 460800: return B460800;
141 | 		default: return 0;
142 | 	}
143 | }
144 | 
145 | 
146 | /**
147 |  * Open serial device
148 |  *
149 |  * @param *device		Path to serial device, e.g. /dev/ttyS0
150 |  *
151 |  * @return 0 on success, else -1
152 |  */
153 | 
154 | int serial_open( const void *params )
155 | {
156 | 	ser_params_t *serial = (ser_params_t *) params;
157 |     struct termios settings;
158 |     tcflag_t bitrate;
159 | 
160 |     // Convert bitrate to flag
161 |     bitrate = __bitrate_to_flag( serial->bitrate );
162 |     if ( bitrate == 0 )
163 |     {
164 | 		fprintf( stderr, "Invalid bitrate '%d' for serial device\n", serial->bitrate );
165 | 		return -1;
166 |     }
167 | 
168 |     // Open serial device
169 | 	conn.fd = open( serial->device, O_RDWR | O_NOCTTY );
170 | 	if ( conn.fd < 0 )
171 | 	{
172 | 		fprintf( stderr, "Failed to open serial device '%s' (errno: %s)\n", serial->device, strerror(errno) );
173 | 		return -1;
174 | 	}
175 | 
176 | 	// Check if device is a terminal device
177 |     if ( !isatty( conn.fd ) )
178 |     {
179 |         fprintf( stderr, "Device '%s' is not a terminal device (errno: %s)!\n", serial->device, strerror(errno) );
180 |         close( conn.fd );
181 |         return -1;
182 |     }
183 | 
184 |     // Set input flags
185 |     settings.c_iflag =  IGNBRK          // Ignore BREAKS on Input
186 |                      |  IGNPAR;         // No Parity
187 |     									// ICRNL: map CR to NL (otherwise a CR input on the other computer will not terminate input)
188 | 
189 |     // Set output flags
190 |     settings.c_oflag = 0;				// Raw output
191 | 
192 |     // Set controlflags
193 |     settings.c_cflag = bitrate
194 |     				 | CS8              // 8 bits per byte
195 |     				 | CSTOPB			// Stop bit
196 |                      | CREAD            // characters may be read
197 |                      | CLOCAL;          // ignore modem state, local connection
198 | 
199 |     // Set local flags
200 |     settings.c_lflag = 0;				// Other option: ICANON = enable canonical input
201 | 
202 |     // Set maximum wait time on input - cf. Linux Serial Programming HowTo, non-canonical mode
203 |     // http://tldp.org/HOWTO/Serial-Programming-HOWTO/x115.html
204 |     settings.c_cc[VTIME] = 10;			// 0 means timer is not uses
205 | 
206 |     // Set minimum bytes to read
207 |     settings.c_cc[VMIN]  = 0;			// 1 means wait until at least 1 character is received
208 | 
209 | 	// Now clean the modem line and activate the settings for the port
210 | 	tcflush( conn.fd, TCIFLUSH );
211 | 	tcsetattr( conn.fd, TCSANOW, &settings );
212 | 
213 | 	return(0);
214 | }
215 | 
216 | 
217 | /**
218 |  * Close serial device
219 |  */
220 | 
221 | void serial_close( void )
222 | {
223 | 	close( conn.fd );
224 | }
225 | 
226 | 
227 | /**
228 |  * Read from serial device
229 |  *
230 |  * @param *buf		Pointer to receive buffer
231 |  * @param len		Number of bytes wished to read
232 |  *
233 |  * @return Number of bytes read
234 |  */
235 | 
236 | int serial_read( unsigned char *buf, unsigned int len )
237 | {
238 | 	int res;
239 | 
240 | 	res = read( conn.fd, buf, len );
241 | 	if ( res < 0 )
242 | 	{
243 | 		fprintf( stderr, "Failed to read from serial device\n" );
244 | 		exit(1);
245 | 	}
246 | 
247 | 	return res;
248 | }
249 | 
250 | 
251 | /**
252 |  * Write to serial device
253 |  *
254 |  * @param *buf		Pointer to buffer that holds data to be sent
255 |  * @param len		Number of bytes to send
256 |  *
257 |  * @return Number of bytes written
258 |  */
259 | 
260 | int serial_write( unsigned char *buf, unsigned int len )
261 | {
262 | 	return( write( conn.fd, (void *) buf, len ) );
263 | }
264 | 
265 | 
266 | 


--------------------------------------------------------------------------------
/wsg_50_driver/src/tcp.c:
--------------------------------------------------------------------------------
  1 | //======================================================================
  2 | /**
  3 |  *  @file
  4 |  *  tcp.c
  5 |  *
  6 |  *  @section tcp.c_general General file information
  7 |  *
  8 |  *  @brief
  9 |  *  
 10 |  *
 11 |  *  @author wolfer
 12 |  *  @date	08.07.2011
 13 |  *  
 14 |  *  
 15 |  *  @section tcp.c_copyright Copyright
 16 |  *  
 17 |  *  Copyright 2011 Weiss Robotics, D-71636 Ludwigsburg, Germany
 18 |  *  
 19 |  *  Redistribution and use in source and binary forms, with or without
 20 |  *  modification, are permitted provided that the following conditions are met:
 21 |  *
 22 |  *     * Redistributions of source code must retain the above copyright
 23 |  *       notice, this list of conditions and the following disclaimer.
 24 |  *     * Redistributions in binary form must reproduce the above copyright
 25 |  *       notice, this list of conditions and the following disclaimer in the
 26 |  *       documentation and/or other materials provided with the distribution.
 27 |  *     * Neither the name of the and Weiss Robotics GmbH nor the names of its 
 28 |  *       contributors may be used to endorse or promote products derived from
 29 |  *	 this software without specific prior written permission.
 30 |  *
 31 |  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 32 |  *  AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 33 |  *  IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 34 |  *  ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
 35 |  *  LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 36 |  *  CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 37 |  *  SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 38 |  *  INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 39 |  *  CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 40 |  *  ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 41 |  *  POSSIBILITY OF SUCH DAMAGE.
 42 |  *
 43 |  */
 44 | //======================================================================
 45 | 
 46 | 
 47 | //------------------------------------------------------------------------
 48 | // Includes
 49 | //------------------------------------------------------------------------
 50 | 
 51 | #include <stdio.h>
 52 | #include <stdlib.h>
 53 | #include <string.h>
 54 | 
 55 | #include "wsg_50/interface.h"
 56 | #include "wsg_50/tcp.h"
 57 | 
 58 | 
 59 | //------------------------------------------------------------------------
 60 | // Macros
 61 | //------------------------------------------------------------------------
 62 | 
 63 | #define TCP_RCV_TIMEOUT_SEC					60
 64 | 
 65 | //------------------------------------------------------------------------
 66 | // Typedefs, enums, structs
 67 | //------------------------------------------------------------------------
 68 | 
 69 | 
 70 | //------------------------------------------------------------------------
 71 | // Global variables
 72 | //------------------------------------------------------------------------
 73 | 
 74 | const interface_t tcp =
 75 | {
 76 | 	.name = "tcp",
 77 | 	.open = &tcp_open,
 78 | 	.close = &tcp_close,
 79 | 	.read = &tcp_read,
 80 | 	.write = &tcp_write
 81 | };
 82 | 
 83 | static tcp_conn_t conn;
 84 | 
 85 | 
 86 | //------------------------------------------------------------------------
 87 | // Local function prototypes
 88 | //------------------------------------------------------------------------
 89 | 
 90 | 
 91 | //------------------------------------------------------------------------
 92 | // Unit Testing
 93 | //------------------------------------------------------------------------
 94 | 
 95 | 
 96 | //------------------------------------------------------------------------
 97 | // Function implementation
 98 | //------------------------------------------------------------------------
 99 | 
100 | /**
101 |  * Open TCP socket
102 |  *
103 |  * @param *params		Connection parameters
104 |  *
105 |  * @return 0 on success, else -1
106 |  */
107 | 
108 | int tcp_open( const void *params )
109 | {
110 | 	int res;
111 | 	tcp_params_t *tcp = (tcp_params_t *) params;
112 | 
113 | 	conn.server = tcp->addr;
114 | 
115 | 	conn.sock = socket( PF_INET, SOCK_STREAM, IPPROTO_TCP );
116 | 	if( conn.sock < 0 )
117 | 	{
118 | 		fprintf( stderr, "Cannot open TCP socket\n" );
119 | 		return -1;
120 | 	}
121 | 
122 |     memset( (char *) &conn.si_server, 0, sizeof(conn.si_server) );
123 |     conn.si_server.sin_family = AF_INET;
124 |     conn.si_server.sin_port = htons( tcp->port );
125 |     conn.si_server.sin_addr.s_addr = tcp->addr;
126 | 
127 | 	unsigned int val = 1024;
128 |     setsockopt( conn.sock, SOL_SOCKET, SO_RCVBUF, (void *) &val, (socklen_t) sizeof( val ) );
129 | 
130 |     struct timeval timeout = { .tv_sec = TCP_RCV_TIMEOUT_SEC, .tv_usec = 0 };
131 |     setsockopt( conn.sock, SOL_SOCKET, SO_RCVTIMEO, (void *) &timeout, (socklen_t) sizeof( struct timeval ) );
132 | 
133 |     res = connect( conn.sock, (struct sockaddr *) &conn.si_server, sizeof(conn.si_server) );
134 |     if ( res < 0 ) return -1;
135 | 
136 |     return 0;
137 | }
138 | 
139 | 
140 | /**
141 |  * Close TCP socket
142 |  *
143 |  * @return 0
144 |  */
145 | 
146 | void tcp_close( void )
147 | {
148 | 	close( conn.sock );
149 | 	conn.sock = 0;
150 | }
151 | 
152 | 
153 | /**
154 |  * Read character from TCP socket
155 |  *
156 |  * @return Character read
157 |  */
158 | 
159 | int tcp_read( unsigned char *buf, unsigned int len )
160 | {
161 |     int res;
162 | 
163 |     if ( conn.sock <= 0 || buf == NULL ) return -1;
164 |     if ( len == 0 ) return 0;
165 | 
166 | 	// Read desired number of bytes
167 | 	res = recv( conn.sock, buf, len, 0 );
168 | 	if ( res < 0 )
169 | 	{
170 | 		close( conn.sock );
171 | 		quit( "Failed to read data from TCP socket\n" );
172 | 	}
173 | 
174 |     return res;
175 | }
176 | 
177 | 
178 | /**
179 |  * Write to TCP socket
180 |  *
181 |  * @param *buf		Pointer to buffer that holds data to be sent
182 |  * @param len		Number of bytes to send
183 |  *
184 |  * @return 0 if successful, -1 on failure
185 |  */
186 | 
187 | int tcp_write( unsigned char *buf, unsigned int len )
188 | {
189 |     int res;
190 | 
191 | 	if ( conn.sock <= 0 ) return( -1 );
192 | 
193 | 	res = send( conn.sock, buf, len, 0 );
194 |     if ( res >= 0 ) return( res );
195 |     else
196 |     {
197 |     	fprintf( stderr, "Failed to send data using TCP socket\n" );
198 |     	return -1;
199 |     }
200 | }
201 | 
202 | 
203 | //------------------------------------------------------------------------
204 | // Test implementation
205 | //------------------------------------------------------------------------
206 | 


--------------------------------------------------------------------------------
/wsg_50_driver/src/udp.c:
--------------------------------------------------------------------------------
  1 | //======================================================================
  2 | /**
  3 |  *  @file
  4 |  *  udp.c
  5 |  *
  6 |  *  @section udp.c_general General file information
  7 |  *
  8 |  *  @brief
  9 |  *  
 10 |  *
 11 |  *  @author wolfer
 12 |  *  @date	07.07.2011
 13 |  *  
 14 |  *  
 15 |  *  @section udp.c_copyright Copyright
 16 |  *  
 17 |  *  Copyright 2011 Weiss Robotics, D-71636 Ludwigsburg, Germany
 18 |  *  
 19 |  *  Redistribution and use in source and binary forms, with or without
 20 |  *  modification, are permitted provided that the following conditions are met:
 21 |  *
 22 |  *     * Redistributions of source code must retain the above copyright
 23 |  *       notice, this list of conditions and the following disclaimer.
 24 |  *     * Redistributions in binary form must reproduce the above copyright
 25 |  *       notice, this list of conditions and the following disclaimer in the
 26 |  *       documentation and/or other materials provided with the distribution.
 27 |  *     * Neither the name of the and Weiss Robotics GmbH nor the names of its 
 28 |  *       contributors may be used to endorse or promote products derived from
 29 |  *	 this software without specific prior written permission.
 30 |  *
 31 |  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 32 |  *  AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 33 |  *  IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 34 |  *  ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
 35 |  *  LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 36 |  *  CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 37 |  *  SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 38 |  *  INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 39 |  *  CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 40 |  *  ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 41 |  *  POSSIBILITY OF SUCH DAMAGE.
 42 |  *
 43 |  */
 44 | //======================================================================
 45 | 
 46 | 
 47 | //------------------------------------------------------------------------
 48 | // Includes
 49 | //------------------------------------------------------------------------
 50 | 
 51 | #include <stdio.h>
 52 | #include <stdlib.h>
 53 | #include <string.h>
 54 | 
 55 | #include "wsg_50/interface.h"
 56 | #include "wsg_50/udp.h"
 57 | 
 58 | 
 59 | //------------------------------------------------------------------------
 60 | // Macros
 61 | //------------------------------------------------------------------------
 62 | 
 63 | 
 64 | //------------------------------------------------------------------------
 65 | // Typedefs, enums, structs
 66 | //------------------------------------------------------------------------
 67 | 
 68 | 
 69 | //------------------------------------------------------------------------
 70 | // Global variables
 71 | //------------------------------------------------------------------------
 72 | 
 73 | const interface_t udp =
 74 | {
 75 | 	.name = "udp",
 76 | 	.open = &udp_open,
 77 | 	.close = &udp_close,
 78 | 	.read = &udp_read,
 79 | 	.write = &udp_write
 80 | };
 81 | 
 82 | static udp_conn_t conn;
 83 | 
 84 | 
 85 | //------------------------------------------------------------------------
 86 | // Local function prototypes
 87 | //------------------------------------------------------------------------
 88 | 
 89 | 
 90 | //------------------------------------------------------------------------
 91 | // Function implementation
 92 | //------------------------------------------------------------------------
 93 | 
 94 | /**
 95 |  * Open UDP socket
 96 |  *
 97 |  * @param *params		Connection parameters
 98 |  *
 99 |  * @return 0 on success, else -1
100 |  */
101 | 
102 | int udp_open( const void *params )
103 | {
104 | 	udp_params_t *udp = (udp_params_t *) params;
105 | 
106 | 	conn.server = udp->addr;
107 | 
108 | 	conn.rcv_bufptr = 0;
109 | 	conn.rcv_bufsize = UDP_RCV_BUFSIZE;
110 | 	
111 | 	conn.sock = socket( PF_INET, SOCK_DGRAM, IPPROTO_UDP );
112 | 	if( conn.sock < 0 )
113 | 	{
114 | 		fprintf( stderr, "Cannot open UDP socket\n" );
115 | 		return -1;
116 | 	}
117 | 
118 |     memset( (char *) &conn.si_server, 0, sizeof(conn.si_server) );
119 |     conn.si_server.sin_family = AF_INET;
120 |     conn.si_server.sin_port = htons( udp->remote_port );
121 |     conn.si_server.sin_addr.s_addr = udp->addr;
122 | 
123 |     conn.si_listen.sin_family = AF_INET;
124 |     conn.si_listen.sin_addr.s_addr = htonl( INADDR_ANY );
125 |     conn.si_listen.sin_port = htons( udp->local_port );
126 | 
127 | 	unsigned int val = UDP_RCV_BUFSIZE;
128 |     setsockopt( conn.sock, SOL_SOCKET, SO_RCVBUF, (void *) &val, (socklen_t) sizeof( val ) );
129 | 
130 |     struct timeval timeout = { .tv_sec = 10, .tv_usec = 0 };
131 |     setsockopt( conn.sock, SOL_SOCKET, SO_RCVTIMEO, (void *) &timeout, (socklen_t) sizeof( struct timeval ) );
132 | 
133 |     if ( bind( conn.sock, (struct sockaddr *) &conn.si_listen, sizeof(conn.si_listen) ) < 0 )
134 |     {
135 |     	fprintf( stderr, "Cannot bind port %d\n", udp->local_port );
136 |     	return -1;
137 |     }
138 | 
139 |     return 0;
140 | }
141 | 
142 | 
143 | /**
144 |  * Close UDP socket
145 |  *
146 |  * @return 0
147 |  */
148 | 
149 | void udp_close( void )
150 | {
151 | 	close( conn.sock );
152 | 	conn.sock = 0;
153 | }
154 | 
155 | 
156 | /**
157 |  * Read character from UDP socket
158 |  *
159 |  * Important note:
160 |  * UDP works with datagrams and not with streams.
161 |  * This means that recvfrom() gets the whole datagram
162 |  * and dumps any data that exceeds the desired length!
163 |  * This is why we check the size of an incoming datagram
164 |  * before doing recvfrom() and read it out as a whole
165 |  * into some buffer that is big enough to hold even large
166 |  * datagrams.
167 |  * If none-requested data is left in the buffer, subsequent
168 |  * read calls will take the data from the buffer until it's
169 |  * empty rather than getting new data from the net.
170 |  *
171 |  * @param *buf		Pointer to input buffer
172 |  * @param len		Number of bytes that should be read
173 |  *
174 |  * @return Number of characters read
175 |  */
176 | 
177 | int udp_read( unsigned char *buf, unsigned int len )
178 | {
179 | 	fd_set readfds;
180 | 	unsigned int bytes_left;
181 |     int res,
182 |        	incoming,
183 |     	packsize,
184 |     	slen = sizeof( conn.si_incoming );
185 | 
186 |     if ( conn.sock <= 0 || buf == NULL )
187 |     {
188 |     	fprintf( stderr, "Parameter error (sock=%d, buf=%p)\n", conn.sock, buf );
189 |     	return -1;
190 |     }
191 | 
192 |     if ( len == 0 ) return 0;
193 | 
194 |     if ( conn.rcv_bufptr == 0 )
195 |     {
196 |     	conn.rcv_bufsize = UDP_RCV_BUFSIZE;
197 | 
198 |     	// Wait for packet to arrive
199 |     	FD_ZERO( &readfds );
200 |     	FD_SET( conn.sock, &readfds );
201 |         struct timeval timeout;
202 |         timeout.tv_sec = 0; timeout.tv_usec = 150000;
203 |         res = select( conn.sock + 1, &readfds, NULL, NULL, &timeout);
204 |         if ( res == 0 ) return -1; /* timeout */
205 |         if ( res < 0 ) return -1;  /* error */
206 | 
207 |     	// Get size of packet pending
208 |         res = ioctl( conn.sock, FIONREAD, &packsize );
209 |         if ( res < 0 ) return -1;
210 | 
211 |         // Check if buffer is big enough to hold datagram
212 |         if ( packsize > UDP_RCV_BUFSIZE )
213 |         {
214 |         	fprintf( stderr, "UDP buffer too small for incoming datagram" );
215 |         	return -1;
216 |         }
217 | 
218 |         // Read packet non-blocking
219 | 		incoming = recvfrom( conn.sock, conn.rcv_buf, packsize, MSG_DONTWAIT, (struct sockaddr *) &conn.si_incoming, (socklen_t *) &slen );
220 | 		if ( incoming < 0 )
221 | 		{
222 | 			fprintf( stderr, "recvfrom() returned error (%d)\n", incoming );
223 | 			return -1;
224 | 		}
225 | 		if ( conn.si_incoming.sin_addr.s_addr != conn.server )
226 | 		{
227 | 			fprintf( stderr, "Message from unknown server!\n" );
228 | 			return -1;
229 | 		}
230 | 		
231 | 		conn.rcv_bufsize = (unsigned int) incoming;
232 |     }
233 | 
234 |     bytes_left = conn.rcv_bufsize - conn.rcv_bufptr;
235 |     if ( len < bytes_left )
236 |     {
237 |     	memcpy( buf, &conn.rcv_buf[conn.rcv_bufptr], len );
238 |     	conn.rcv_bufptr += len;
239 |     	res = (int) len;
240 |     }
241 |     else
242 |     {
243 |     	memcpy( buf, &conn.rcv_buf[conn.rcv_bufptr], len );
244 |     	conn.rcv_bufptr = 0;
245 |     	res = (int) bytes_left;
246 |     }
247 | 
248 |     return res;
249 | }
250 | 
251 | 
252 | /**
253 |  * Write to UDP socket
254 |  *
255 |  * @param *buf		Pointer to buffer that holds data to be sent
256 |  * @param len		Number of bytes to send
257 |  *
258 |  * @return 0 if successful, -1 on failure
259 |  */
260 | 
261 | int udp_write( unsigned char *buf, unsigned int len )
262 | {
263 |     int res,
264 |     	slen = sizeof( conn.si_incoming );
265 | 
266 | 	if ( conn.sock <= 0 ) return( -1 );
267 | 
268 | 	res = sendto( conn.sock, buf, len, 0, (struct sockaddr *) &conn.si_server, (socklen_t) slen );
269 |     if ( res >= 0 ) return res;
270 |     else return -1;
271 | }
272 | 


--------------------------------------------------------------------------------
/wsg_50_simulation/CMakeLists.txt:
--------------------------------------------------------------------------------
 1 | cmake_minimum_required(VERSION 2.8.3)
 2 | project(wsg_50_simulation)
 3 | 
 4 | ## Find catkin macros and libraries
 5 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
 6 | ## is used, also find other catkin packages
 7 | find_package(catkin REQUIRED COMPONENTS
 8 |   roscpp
 9 |   std_msgs
10 |   std_srvs
11 |   urdf
12 |   wsg_50_common
13 | )
14 | 
15 | catkin_package(
16 | #   INCLUDE_DIRS include
17 | #  LIBRARIES vh_pixelmap
18 |   CATKIN_DEPENDS roscpp urdf std_msgs std_srvs wsg_50_common
19 | #  DEPENDS system_lib
20 | )
21 | 
22 | ## System dependencies are found with CMake's conventions
23 | # find_package(Boost REQUIRED COMPONENTS system)
24 | 
25 | include_directories(
26 |   ${catkin_INCLUDE_DIRS}
27 | )
28 | 
29 | add_executable(wsg_50_sim_keyboard_teleop src/wsg_50_keyboard_teleop.cpp)
30 | target_link_libraries(wsg_50_sim_keyboard_teleop ${catkin_LIBRARIES})
31 | add_dependencies(wsg_50_sim_keyboard_teleop wsg_50_common_generate_messages_cpp)
32 | 
33 | add_executable(wsg_50_sim_driver src/wsg_50_sim_driver.cpp)
34 | target_link_libraries(wsg_50_sim_driver ${catkin_LIBRARIES})
35 | add_dependencies(wsg_50_sim_driver wsg_50_common_generate_messages_cpp)
36 | 


--------------------------------------------------------------------------------
/wsg_50_simulation/controllers/wsg_50_gl.yaml:
--------------------------------------------------------------------------------
1 | wsg_50_gl:
2 |   type: robot_mechanism_controllers/JointPositionController
3 |   joint: base_joint_gripper_left
4 |   pid:
5 |     p: 100.0
6 |     d: 10.0
7 | 
8 | 


--------------------------------------------------------------------------------
/wsg_50_simulation/controllers/wsg_50_gl_modular.yaml:
--------------------------------------------------------------------------------
1 | wsg_50_gl_modular:
2 |   type: robot_mechanism_controllers/JointPositionController
3 |   joint: wsg_50_modular_base_joint_gripper_left
4 |   pid:
5 |     p: 100.0
6 |     d: 10.0
7 | 
8 | 


--------------------------------------------------------------------------------
/wsg_50_simulation/controllers/wsg_50_gl_powerball.yaml:
--------------------------------------------------------------------------------
1 | wsg_50_gl_powerball:
2 |   type: robot_mechanism_controllers/JointPositionController
3 |   joint: wsg_50_powerball_base_joint_gripper_left
4 |   pid:
5 |     p: 100.0
6 |     d: 10.0
7 | 
8 | 


--------------------------------------------------------------------------------
/wsg_50_simulation/controllers/wsg_50_gr.yaml:
--------------------------------------------------------------------------------
1 | wsg_50_gr:
2 |   type: robot_mechanism_controllers/JointPositionController
3 |   joint: base_joint_gripper_right
4 |   pid:
5 |     p: 100.0
6 |     d: 10.0
7 | 
8 | 


--------------------------------------------------------------------------------
/wsg_50_simulation/controllers/wsg_50_gr_modular.yaml:
--------------------------------------------------------------------------------
1 | wsg_50_gr_modular:
2 |   type: robot_mechanism_controllers/JointPositionController
3 |   joint: wsg_50_modular_base_joint_gripper_right
4 |   pid:
5 |     p: 100.0
6 |     d: 10.0
7 | 
8 | 


--------------------------------------------------------------------------------
/wsg_50_simulation/controllers/wsg_50_gr_powerball.yaml:
--------------------------------------------------------------------------------
1 | wsg_50_gr_powerball:
2 |   type: robot_mechanism_controllers/JointPositionController
3 |   joint: wsg_50_powerball_base_joint_gripper_right
4 |   pid:
5 |     p: 100.0
6 |     d: 10.0
7 | 
8 | 


--------------------------------------------------------------------------------
/wsg_50_simulation/launch/wsg_50.launch:
--------------------------------------------------------------------------------
 1 | <?xml version="1.0"?>
 2 | <launch>
 3 | 
 4 |   <param name="/use_sim_time" value="true" />
 5 | 
 6 |   <!-- start world -->
 7 |   <include file="$(find gazebo_ros)/launch/empty_world.launch"/>
 8 |  
 9 |   <!-- send wsg_50.urdf to param server -->
10 |   <param name="robot_description" textfile="$(find wsg_50_simulation)/urdf/wsg_50.urdf" />
11 | 
12 |   <!-- push gripper_description to factory and spawn the gripper in gazebo -->
13 |   <node name="spawn_gripper" pkg="gazebo_ros" type="spawn_model" args="-urdf -param robot_description -x 0 -y 0 -z 0.35 -model wsg_50" respawn="false" output="screen"/>
14 | 
15 |   <!-- Launch the joint controller -->
16 |   <include file="$(find wsg_50_simulation)/launch/wsg_50_controllers.launch"/>
17 | 
18 |   <!-- Launch de joint state publisher, necessary to define the gripper dependency relation -->
19 |   <param name="use_gui" value="True"/>
20 |   <rosparam command="load" file="$(find wsg_50_simulation)/urdf/wsg_50_dependent_joints.yaml"/>
21 |   <!--node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher"/-->
22 |   <node name="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher" />
23 |   
24 |   <!-- Launch the fake driver to provide the same services that in the real gripper -->
25 |   <node name="wsg_50" pkg="wsg_50_simulation" type="wsg_50_sim_driver">
26 | 	<param name = "vel_pub_l_Topic" type="string" value="/wsg_50_gl/command"/>
27 | 	<param name = "vel_pub_r_Topic" type="string" value="/wsg_50_gr/command"/>
28 |   </node>
29 | 
30 |   <!--LAUNCH RVIZ -->	
31 |   <!--node name="rviz" pkg="rviz" type="rviz" args="-d $(find urdf_tutorial)/urdf.vcg" /-->
32 | 
33 | </launch>
34 | 


--------------------------------------------------------------------------------
/wsg_50_simulation/launch/wsg_50_controllers.launch:
--------------------------------------------------------------------------------
 1 | <?xml version="1.0"?>
 2 | <launch>
 3 | 
 4 |   <!-- Launch the joint controller -->
 5 |   <rosparam file="$(find wsg_50_simulation)/controllers/wsg_50_gr.yaml" command="load" />
 6 |   <node pkg="pr2_controller_manager" type="spawner" args="wsg_50_gr" name="wsg_50_gr" />
 7 | 
 8 |   <rosparam file="$(find wsg_50_simulation)/controllers/wsg_50_gl.yaml" command="load" />
 9 |   <node pkg="pr2_controller_manager" type="spawner" args="wsg_50_gl" name="wsg_50_gl" />
10 | 
11 | </launch>
12 | 


--------------------------------------------------------------------------------
/wsg_50_simulation/launch/wsg_50_modular_controllers.launch:
--------------------------------------------------------------------------------
 1 | <?xml version="1.0"?>
 2 | <launch>
 3 | 
 4 |   <!-- Launch the joint controller -->
 5 |   <rosparam file="$(find wsg_50_simulation)/controllers/wsg_50_gr_modular.yaml" command="load" />
 6 |   <node pkg="pr2_controller_manager" type="spawner" args="wsg_50_gr_modular" name="wsg_50_gr_modular" />
 7 | 
 8 |   <rosparam file="$(find wsg_50_simulation)/controllers/wsg_50_gl_modular.yaml" command="load" />
 9 |   <node pkg="pr2_controller_manager" type="spawner" args="wsg_50_gl_modular" name="wsg_50_gl_modular" />
10 | 
11 | </launch>
12 | 


--------------------------------------------------------------------------------
/wsg_50_simulation/launch/wsg_50_powerball_controllers.launch:
--------------------------------------------------------------------------------
 1 | <?xml version="1.0"?>
 2 | <launch>
 3 | 
 4 |   <!-- Launch the joint controller -->
 5 |   <rosparam file="$(find wsg_50_simulation)/controllers/wsg_50_gr_powerball.yaml" command="load" />
 6 |   <node pkg="pr2_controller_manager" type="spawner" args="wsg_50_gr_powerball" name="wsg_50_gr_powerball" />
 7 | 
 8 |   <rosparam file="$(find wsg_50_simulation)/controllers/wsg_50_gl_powerball.yaml" command="load" />
 9 |   <node pkg="pr2_controller_manager" type="spawner" args="wsg_50_gl_powerball" name="wsg_50_gl_powerball" />
10 | 
11 | </launch>
12 | 


--------------------------------------------------------------------------------
/wsg_50_simulation/launch/wsg_50_teleop.launch:
--------------------------------------------------------------------------------
1 | <?xml version="1.0"?>
2 | <launch>
3 | 
4 |   <!-- Launch the node for keyboard teleoperation -->
5 |   <node name="wsg_50_sim_keyboard_teleop" pkg="wsg_50_simulation" type="wsg_50_sim_keyboard_teleop"/>
6 | 
7 | </launch>
8 | 


--------------------------------------------------------------------------------
/wsg_50_simulation/meshes/GUIDE_WSG50_110.stl:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/nalt/wsg50-ros-pkg/bca5f46898d11b27d27c7f63f5c1fafcaf4b8294/wsg_50_simulation/meshes/GUIDE_WSG50_110.stl


--------------------------------------------------------------------------------
/wsg_50_simulation/meshes/WSG-FMF.stl:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/nalt/wsg50-ros-pkg/bca5f46898d11b27d27c7f63f5c1fafcaf4b8294/wsg_50_simulation/meshes/WSG-FMF.stl


--------------------------------------------------------------------------------
/wsg_50_simulation/meshes/WSG50_110.stl:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/nalt/wsg50-ros-pkg/bca5f46898d11b27d27c7f63f5c1fafcaf4b8294/wsg_50_simulation/meshes/WSG50_110.stl


--------------------------------------------------------------------------------
/wsg_50_simulation/package.xml:
--------------------------------------------------------------------------------
 1 | <?xml version="1.0"?>
 2 | <package>
 3 |   <name>wsg_50_simulation</name>
 4 |   <version>0.0.0</version>
 5 |   <description>The wsg_50_simulation package</description>
 6 | 
 7 |   <!-- One maintainer tag required, multiple allowed, one person per tag --> 
 8 |   <!-- Example:  -->
 9 |   <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
10 |   <maintainer email="nalt@todo.todo">nalt</maintainer>
11 | 
12 | 
13 |   <!-- One license tag required, multiple allowed, one license per tag -->
14 |   <!-- Commonly used license strings: -->
15 |   <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
16 |   <license>BSD</license>
17 | 
18 | 
19 |   <!-- Url tags are optional, but mutiple are allowed, one per tag -->
20 |   <!-- Optional attribute type can be: website, bugtracker, or repository -->
21 |   <!-- Example: -->
22 |   <!-- <url type="website">http://wiki.ros.org/wsg_50</url> -->
23 | 
24 | 
25 |   <!-- Author tags are optional, mutiple are allowed, one per tag -->
26 |   <!-- Authors do not have to be maintianers, but could be -->
27 |   <!-- Example: -->
28 |   <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
29 |   <author >Nicolas Alt</author>
30 |   <author >Robotnik</author>
31 |   <author >Weiss Robotics</author>
32 | 
33 | 
34 |   <!-- The *_depend tags are used to specify dependencies -->
35 |   <!-- Dependencies can be catkin packages or system dependencies -->
36 |   <!-- Examples: -->
37 |   <!-- Use build_depend for packages you need at compile time: -->
38 |   <!--   <build_depend>message_generation</build_depend> -->
39 |   <!-- Use buildtool_depend for build tool packages: -->
40 |   <!--   <buildtool_depend>catkin</buildtool_depend> -->
41 |   <!-- Use run_depend for packages you need at runtime: -->
42 |   <!--   <run_depend>message_runtime</run_depend> -->
43 |   <!-- Use test_depend for packages you need only for testing: -->
44 |   <!--   <test_depend>gtest</test_depend> -->
45 |   <buildtool_depend>catkin</buildtool_depend>
46 |   <build_depend>roscpp</build_depend>
47 |   <build_depend>std_msgs</build_depend>
48 |   <build_depend>std_srvs</build_depend>
49 |   <build_depend>wsg_50_common</build_depend>
50 |   <build_depend>urdf</build_depend>
51 |   <run_depend>roscpp</run_depend>
52 |   <run_depend>urdf</run_depend>
53 |   <run_depend>std_msgs</run_depend>
54 |   <run_depend>std_srvs</run_depend>
55 |   <run_depend>wsg_50_common</run_depend>
56 |   <run_depend>pr2_controller_manager</run_depend>
57 |   <run_depend>gazebo_ros</run_depend>
58 | 
59 | 
60 |   <!-- The export tag contains other, unspecified, tags -->
61 |   <export>
62 |     <!-- You can specify that this package is a metapackage here: -->
63 |     <!-- <metapackage/> -->
64 | 
65 |     <!-- Other tools can request additional information be placed here -->
66 | 
67 |   </export>
68 | </package>


--------------------------------------------------------------------------------
/wsg_50_simulation/src/wsg_50_keyboard_teleop.cpp:
--------------------------------------------------------------------------------
  1 | /* wsg_50_keyboard_teleop
  2 |  * Copyright (c) 2012, Robotnik Automation, SLL
  3 |  * All rights reserved.
  4 |  *
  5 |  * Redistribution and use in source and binary forms, with or without
  6 |  * modification, are permitted provided that the following conditions are met:
  7 |  *
  8 |  *     * Redistributions of source code must retain the above copyright
  9 |  *       notice, this list of conditions and the following disclaimer.
 10 |  *     * Redistributions in binary form must reproduce the above copyright
 11 |  *       notice, this list of conditions and the following disclaimer in the
 12 |  *       documentation and/or other materials provided with the distribution.
 13 |  *     * Neither the name of the Robotnik Automation, SLL. nor the names of its
 14 |  *       contributors may be used to endorse or promote products derived from
 15 |  *       this software without specific prior written permission.
 16 |  *
 17 |  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 18 |  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 19 |  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 20 |  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
 21 |  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 22 |  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 23 |  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 24 |  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 25 |  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 26 |  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 27 |  * POSSIBILITY OF SUCH DAMAGE.
 28 |  *
 29 |  * \author Marc Benetó (mbeneto@robotnik.es)
 30 |  * \brief WSG-50 keyboard teleop
 31 |  */
 32 | 
 33 | #include <termios.h>
 34 | #include <signal.h>
 35 | #include <math.h>
 36 | #include <stdio.h>
 37 | #include <stdlib.h>
 38 | 
 39 | #include <ros/ros.h>
 40 | #include <std_msgs/Float64.h>
 41 | 
 42 | #define KEYCODE_A 0x61
 43 | #define KEYCODE_D 0x64
 44 | #define KEYCODE_S 0x73
 45 | #define KEYCODE_W 0x77 
 46 | #define KEYCODE_Q 0x71
 47 | #define KEYCODE_E 0x65
 48 | #define KEYCODE_SPACEBAR 0x49
 49 | 
 50 | #define MAX_GRIPPER_OPEN 0.056
 51 | #define MIN_GRIPPER_OPEN 0.0
 52 | 
 53 | 
 54 | class Wsg50Teleop
 55 | {
 56 |   private:
 57 |   double open_increment, close_increment, grasp_increment, force;
 58 |   std_msgs::Float64 cmd;
 59 | 
 60 |   ros::NodeHandle n_;
 61 |   ros::Publisher vel_pub_r_, vel_pub_l_;
 62 | 
 63 |   public:
 64 |   void init()
 65 |   { 
 66 |     cmd.data = 0;
 67 | 
 68 |     vel_pub_r_ = n_.advertise<std_msgs::Float64>("/wsg_50_gr/command", 1);
 69 |     vel_pub_l_ = n_.advertise<std_msgs::Float64>("/wsg_50_gl/command", 1);
 70 | 
 71 |     ros::NodeHandle n_private("~");
 72 |     n_private.param("open_increment", open_increment, 0.001);
 73 |   }
 74 |   
 75 |   ~Wsg50Teleop()   { }
 76 |   void keyboardLoop();
 77 | 
 78 | };
 79 | 
 80 | int kfd = 0;
 81 | struct termios cooked, raw;
 82 | float currentPos;
 83 | 
 84 | void quit(int sig)
 85 | {
 86 |   tcsetattr(kfd, TCSANOW, &cooked);
 87 |   exit(0);
 88 | }
 89 | 
 90 | int main(int argc, char** argv)
 91 | {
 92 |   ros::init(argc, argv, "wsg_50_teleop");
 93 | 
 94 |   Wsg50Teleop tpk;
 95 |   tpk.init();
 96 | 
 97 |   signal(SIGINT,quit);
 98 | 
 99 |   tpk.keyboardLoop();
100 | 
101 |   return(0);
102 | }
103 | 
104 | void Wsg50Teleop::keyboardLoop()
105 | {
106 |   char c;
107 |   bool dirty=false;
108 |   currentPos = 0.0;
109 | 
110 |   // get the console in raw mode
111 |   tcgetattr(kfd, &cooked);
112 |   memcpy(&raw, &cooked, sizeof(struct termios));
113 |   raw.c_lflag &=~ (ICANON | ECHO);
114 |   // Setting a new line, then end of file
115 |   raw.c_cc[VEOL] = 1;
116 |   raw.c_cc[VEOF] = 2;
117 |   tcsetattr(kfd, TCSANOW, &raw);
118 | 
119 |   puts("Reading from keyboard");
120 |   puts("---------------------------");
121 |   puts("Use 'W' to oppen the gripper");
122 |   puts("Use 'S' to close the gripper");
123 | 
124 |   for(;;)
125 |   {
126 |     // get the next event from the keyboard
127 |     if(read(kfd, &c, 1) < 0)
128 |     {
129 |       perror("read():");
130 |       exit(-1);
131 |     }
132 |     cmd.data = currentPos;
133 | 
134 |     switch(c)
135 |     {
136 | 
137 |     case KEYCODE_W: // Open gripper
138 |       if (currentPos < MAX_GRIPPER_OPEN){
139 |       	currentPos = currentPos + open_increment;
140 |       	cmd.data = currentPos;
141 |       	dirty = true;
142 |       	break;
143 |       }
144 |     case KEYCODE_S: // Close gripper
145 |       if (currentPos > MIN_GRIPPER_OPEN){
146 | 	currentPos = currentPos - open_increment;
147 | 	cmd.data = currentPos;
148 | 	dirty = true;
149 |         break;
150 |       }
151 |     }
152 |     
153 |     if (dirty == true)
154 |     {
155 |       vel_pub_r_.publish(cmd);
156 |       cmd.data = cmd.data * -1.0; // Adapt for the left gripper
157 |       vel_pub_l_.publish(cmd);
158 |     }
159 | 
160 | 
161 |   }
162 | }
163 | 


--------------------------------------------------------------------------------
/wsg_50_simulation/src/wsg_50_sim_driver.cpp:
--------------------------------------------------------------------------------
  1 | /* wsg_50_sim_driver
  2 |  * Copyright (c) 2012, Robotnik Automation, SLL
  3 |  * All rights reserved.
  4 |  *
  5 |  * Redistribution and use in source and binary forms, with or without
  6 |  * modification, are permitted provided that the following conditions are met:
  7 |  *
  8 |  *     * Redistributions of source code must retain the above copyright
  9 |  *       notice, this list of conditions and the following disclaimer.
 10 |  *     * Redistributions in binary form must reproduce the above copyright
 11 |  *       notice, this list of conditions and the following disclaimer in the
 12 |  *       documentation and/or other materials provided with the distribution.
 13 |  *     * Neither the name of the Robotnik Automation, SLL. nor the names of its
 14 |  *       contributors may be used to endorse or promote products derived from
 15 |  *       this software without specific prior written permission.
 16 |  *
 17 |  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 18 |  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 19 |  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 20 |  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
 21 |  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 22 |  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 23 |  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 24 |  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 25 |  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 26 |  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 27 |  * POSSIBILITY OF SUCH DAMAGE.
 28 |  *
 29 |  * \author Marc Benetó (mbeneto@robotnik.es)
 30 |  * \brief WSG-50 sim driver.
 31 |  */
 32 | 
 33 | #include <ros/ros.h>
 34 | #include <wsg_50_common/Move.h>
 35 | #include <wsg_50_common/Incr.h>
 36 | #include <std_msgs/Float64.h>
 37 | #include <std_srvs/Empty.h>
 38 | 
 39 | #define GRIPPER_MAX_OPEN 110.0
 40 | #define GRIPPER_MIN_OPEN 0.0
 41 | 
 42 | using namespace std;
 43 | 
 44 | ros::Publisher vel_pub_r_, vel_pub_l_;
 45 | ros::ServiceClient moveSC;
 46 | double currentOpenning;
 47 | 
 48 | void move(double width){
 49 | 	
 50 | 		double open = width / 2;
 51 | 		
 52 | 		std_msgs::Float64 lCommand, rCommand;
 53 | 		
 54 | 		rCommand.data = open/1000;
 55 | 		lCommand.data = rCommand.data * -1.0;
 56 | 		
 57 | 		vel_pub_r_.publish(rCommand);
 58 | 		vel_pub_l_.publish(lCommand);
 59 | 		
 60 | 		currentOpenning = width;
 61 | 	
 62 | }
 63 | 
 64 | bool moveSrv(wsg_50_common::Move::Request &req, wsg_50_common::Move::Response &res)
 65 | {
 66 | 	if ( req.width >= 0.0 && req.width <= 110.0 ){
 67 |   		ROS_INFO("Moving to %f position.", req.width);
 68 | 		move(req.width);
 69 | 		
 70 | 	}else if (req.width < 0.0 || req.width > 110.0){
 71 | 		ROS_ERROR("Imposible to move to this position. (Width values: [0.0 - 110.0] ");
 72 | 		return false;
 73 | 	}
 74 | 
 75 | 	ROS_INFO("Target position reached.");
 76 |   	return true;
 77 | }
 78 | 
 79 | bool moveIncrementallySrv(wsg_50_common::Incr::Request &req, wsg_50_common::Incr::Response &res)
 80 | {
 81 | 				
 82 | 	if (req.direction == "open"){
 83 | 		
 84 | 		float nextWidth = currentOpenning + req.increment;
 85 | 		
 86 | 		if ( (currentOpenning < GRIPPER_MAX_OPEN) && nextWidth < GRIPPER_MAX_OPEN ){
 87 | 			move(nextWidth);
 88 | 		}else if( nextWidth >= GRIPPER_MAX_OPEN){
 89 | 			move(nextWidth);
 90 | 			currentOpenning = GRIPPER_MAX_OPEN;
 91 | 		}
 92 | 
 93 | 	}else if (req.direction == "close"){
 94 | 
 95 | 		float nextWidth = currentOpenning - req.increment;
 96 | 		
 97 | 		if ( (currentOpenning > GRIPPER_MIN_OPEN) && nextWidth > GRIPPER_MIN_OPEN ){
 98 | 			move(nextWidth);
 99 | 		}else if( nextWidth <= GRIPPER_MIN_OPEN){
100 | 			move(nextWidth);
101 | 			currentOpenning = GRIPPER_MIN_OPEN;
102 | 		}
103 | 	}
104 | 	
105 | }
106 | 
107 | 
108 | bool homingSrv(std_srvs::Empty::Request &req, std_srvs::Empty::Request &res)
109 | {
110 | 	ROS_INFO("Homing...");
111 | 	
112 | 	move(0.0);
113 | 	
114 | 	ROS_INFO("Home position reached.");
115 | 	return true;
116 | }
117 | 
118 | bool graspSrv(wsg_50_common::Move::Request &req, wsg_50_common::Move::Request &res)
119 | {
120 | 	ROS_INFO("Grasping...");
121 | 	
122 | 	// TODO: Increase finger force
123 | 	move(0.0);
124 | 	
125 | 	ROS_INFO("Object grasped");
126 | 	return true;
127 | }
128 | 
129 | 
130 | int main(int argc, char** argv){
131 | 	
132 | 	ros::init(argc, argv, "wsg_50_sim_driver");
133 | 
134 | 	ros::NodeHandle nh("~");
135 | 	
136 | 	std::string vel_pub_l_Topic, vel_pub_r_Topic;
137 | 	
138 | 	nh.param<std::string>("vel_pub_l_Topic", vel_pub_l_Topic, "/wsg_50_gl/command");
139 | 	nh.param<std::string>("vel_pub_r_Topic", vel_pub_r_Topic, "/wsg_50_gr/command");
140 | 	
141 |     currentOpenning = 0.0;
142 | 	
143 | 	ros::ServiceServer moveSS = nh.advertiseService("move", moveSrv);
144 | 	ros::ServiceServer moveIncrementallySS = nh.advertiseService("move_incrementally", moveIncrementallySrv);
145 | 	ros::ServiceServer homingSS = nh.advertiseService("homing", homingSrv);
146 | 	ros::ServiceServer graspSS = nh.advertiseService("grasp", graspSrv);
147 | 	
148 | 	vel_pub_l_ = nh.advertise<std_msgs::Float64>(vel_pub_l_Topic, 1000);
149 | 	vel_pub_r_ = nh.advertise<std_msgs::Float64>(vel_pub_r_Topic, 1000);
150 | 	
151 | 	ros::spin();
152 | 	
153 | } 
154 | 


--------------------------------------------------------------------------------
/wsg_50_simulation/urdf/wsg_50.urdf:
--------------------------------------------------------------------------------
  1 | <?xml version="1.0"?>
  2 | <robot name="wsg_50" 
  3 | 
  4 |        xmlns:xi="http://www.w3.org/2001/XInclude"
  5 |        xmlns:gazebo="http://playerstage.sourceforge.net/gazebo/xmlschema/#gz"
  6 |        xmlns:model="http://playerstage.sourceforge.net/gazebo/xmlschema/#model"
  7 |        xmlns:sensor="http://playerstage.sourceforge.net/gazebo/xmlschema/#sensor"
  8 |        xmlns:body="http://playerstage.sourceforge.net/gazebo/xmlschema/#body"
  9 |        xmlns:geom="http://playerstage.sourceforge.net/gazebo/xmlschema/#geom"
 10 |        xmlns:joint="http://playerstage.sourceforge.net/gazebo/xmlschema/#joint"
 11 |        xmlns:interface="http://playerstage.sourceforge.net/gazebo/xmlschema/#interface"
 12 |        xmlns:rendering="http://playerstage.sourceforge.net/gazebo/xmlschema/#rendering"
 13 |        xmlns:renderable="http://playerstage.sourceforge.net/gazebo/xmlschema/#renderable"
 14 |        xmlns:controller="http://playerstage.sourceforge.net/gazebo/xmlschema/#controller"
 15 |        xmlns:physics="http://playerstage.sourceforge.net/gazebo/xmlschema/#physics">
 16 | 
 17 |  <!-- BASE LINK -->
 18 |  <link name="base_link">
 19 |       <inertial>
 20 |           <mass value="1.2" />
 21 |           <origin xyz="0 0 0" />
 22 |      	  <!--inertia ixx="1.0" ixy="0.0" ixz="0.0"
 23 |                iyy="100.0" iyz="0.0" 
 24 |                izz="1.0" /-->
 25 |       	  <inertia  ixx="1.0" ixy="0.0"  ixz="0.0"  iyy="1.0"  iyz="0.0"  izz="1.0" />
 26 |       </inertial>
 27 | 
 28 |       <visual>
 29 |           <origin xyz="0.0 0 0" rpy="0 0 0" />
 30 |           <geometry>
 31 |              <mesh filename="package://wsg_50_simulation/meshes/WSG50_110.stl" scale="1 1 1"/>
 32 | 	     <!--box size="0.146 0.05 0.0735"/-->
 33 |           </geometry>
 34 |           <material name="grey">
 35 | 		<color rgba="0.5 0.5 0.5 1"/>
 36 |       	  </material>
 37 |       </visual>
 38 | 
 39 |       <collision>
 40 |           <origin xyz="0.0 0 0" rpy="0 0 0" />
 41 |           <geometry>
 42 | 	     <!--box size="0.146 0.05 0.0735"/-->
 43 |              <mesh filename="package://wsg_50_simulation/meshes/WSG50_110.stl" scale="1 1 1"/>
 44 |          </geometry>
 45 |      </collision>
 46 |   </link>
 47 | 
 48 |   <gazebo reference="base_link">
 49 |     <material>Gazebo/Grey</material>
 50 |     <turnGravityOff>false</turnGravityOff>
 51 |   </gazebo>
 52 | 
 53 |   <!-- GRIPPER LEFT -->
 54 | 
 55 |   <joint name="base_joint_gripper_left" type="prismatic">
 56 |      <limit lower="-0.055" upper="-0.0027" effort="1.0" velocity="1.0"/>
 57 |      <origin xyz="0 0 0" rpy="0 0 0" />      <!--origin xyz="-0.0067 0 0.049" rpy="0 0 0" /-->
 58 |      <parent link="base_link"/>
 59 |      <child link="gripper_left" />
 60 |      <dynamics friction="100" damping="100" />
 61 |      <axis xyz="1 0 0"/>
 62 |      <limit effort="100" velocity="100"/>
 63 |   </joint>
 64 | 
 65 |   <transmission name="base_trans_left" type="pr2_mechanism_model/SimpleTransmission">
 66 |      <actuator name="base_motor_left" />
 67 |      <joint name="base_joint_gripper_left" />
 68 |      <mechanicalReduction>1</mechanicalReduction>
 69 |      <motorTorqueConstant>1</motorTorqueConstant>
 70 |   </transmission>
 71 | 
 72 |   <link name="gripper_left">
 73 |       <inertial>
 74 |           <mass value="0.1" />
 75 |           <origin xyz="0 0 0" />
 76 |       <inertia ixx="1.0" ixy="0.0" ixz="0.0"
 77 |                iyy="1.0" iyz="0.0" 
 78 |                izz="1.0" />
 79 |       </inertial>
 80 | 
 81 |       <visual>
 82 |           <origin xyz="0.0 0 0" rpy="0 0 0" />
 83 |           <geometry>
 84 |              <mesh filename="package://wsg_50_simulation/meshes/GUIDE_WSG50_110.stl" scale="0.001 0.001 0.001"/>
 85 | 	     <!--box size="0.0175 0.03 0.024"/-->
 86 |           </geometry>
 87 |           <material name="black">
 88 | 		<color rgba="0 0 0 1"/>
 89 |       	  </material>
 90 |       </visual>
 91 | 
 92 |       <collision>
 93 |           <origin xyz="0 0 0" rpy="0 0 0" />
 94 |           <geometry>
 95 | 	     <!--box size="0.0175 0.03 0.024"/-->
 96 |              <mesh filename="package://wsg_50_simulation/meshes/GUIDE_WSG50_110.stl" scale="0.001 0.001 0.001"/>
 97 |          </geometry>
 98 |      </collision>
 99 |   </link>
100 | 
101 |   <gazebo reference="gripper_left">
102 |     <material>Gazebo/Blue</material>
103 |     <turnGravityOff>false</turnGravityOff>
104 |   </gazebo>
105 | 
106 | 
107 |   <!-- LEFT FINGER -->
108 | 
109 |   <joint name="guide_joint_finger_left" type="fixed">
110 |      <origin xyz="0 0 0.023" rpy="0 0 0" />
111 |      <parent link="gripper_left"/>
112 |      <child link="finger_left" />
113 |      <dynamics friction="100" damping="100" />
114 |      <axis xyz="1 0 0"/>
115 |      <limit effort="100" velocity="100"/>
116 |   </joint>
117 | 
118 |   <link name="finger_left">
119 |       <inertial>
120 |           <mass value="0.1" />
121 |           <origin xyz="0 0 0" />
122 |       <inertia ixx="1.0" ixy="0.0" ixz="0.0"
123 |                iyy="1.0" iyz="0.0" 
124 |                izz="1.0" />
125 |       </inertial>
126 | 
127 |       <visual>
128 |           <origin xyz="0.0 0 0" rpy="0 0 0" />
129 |           <geometry>
130 |              <mesh filename="package://wsg_50_simulation/meshes/WSG-FMF.stl" scale="0.001 0.001 0.001"/>
131 |           </geometry>
132 |           <material name="black">
133 | 		<color rgba="0 0 0 1"/>
134 |       	  </material>
135 |       </visual>
136 | 
137 |       <collision>
138 |           <origin xyz="0 0 0" rpy="0 0 0" />
139 |           <geometry>
140 |              <mesh filename="package://wsg_50_simulation/meshes/WSG-FMF.stl" scale="0.001 0.001 0.001"/>
141 |          </geometry>
142 |      </collision>
143 |   </link>
144 | 
145 |   <gazebo reference="finger_left">
146 |     <material>Gazebo/Blue</material>
147 |     <turnGravityOff>false</turnGravityOff>
148 |   </gazebo>
149 | 
150 | 
151 |   <!-- GRIPPER RIGHT -->
152 | 
153 |   <joint name="base_joint_gripper_right" type="prismatic">
154 |      <limit lower="0.0027" upper="0.055" effort="1.0" velocity="1.0"/>
155 |      <origin xyz="0 0 0" rpy="0 0 3.14159" />
156 |      <parent link="base_link"/>
157 |      <child link="gripper_right" />
158 |      <axis xyz="-1 0 0"/>
159 |      <dynamics friction="100" damping="100" />
160 |      <limit effort="100" velocity="100"/>
161 |   </joint>
162 | 
163 |   <transmission name="base_trans_right" type="pr2_mechanism_model/SimpleTransmission">
164 |      <actuator name="base_motor_right" />
165 |      <joint name="base_joint_gripper_right" />
166 |      <mechanicalReduction>1</mechanicalReduction>
167 |      <motorTorqueConstant>1</motorTorqueConstant>
168 |   </transmission>
169 | 	
170 |     <link name="gripper_right">
171 |       <inertial>
172 |           <mass value="0.1" />
173 |           <origin xyz="0 0 0" />
174 |       <inertia ixx="1.0" ixy="0.0" ixz="0.0"
175 |                iyy="1.0" iyz="0.0" 
176 |                izz="1.0" />
177 |       </inertial>
178 | 
179 |       <visual>
180 |           <origin xyz="0.0 0 0" rpy="0 0 0" />
181 |           <geometry>
182 |              <mesh filename="package://wsg_50_simulation/meshes/GUIDE_WSG50_110.stl" scale="0.001 0.001 0.001"/>
183 | 	     <!--box size="0.0175 0.03 0.024"/-->
184 |           </geometry>
185 |           <material name="black">
186 | 		<color rgba="0 0 0 1"/>
187 |       	  </material>
188 |       </visual>
189 | 
190 |       <collision>
191 |           <origin xyz="0 0 0" rpy="0 0 0" />
192 |           <geometry>
193 | 	     <!--box size="0.0175 0.03 0.024"/-->
194 |              <mesh filename="package://wsg_50_simulation/meshes/GUIDE_WSG50_110.stl" scale="0.001 0.001 0.001"/>
195 |          </geometry>
196 |      </collision>
197 |   </link>
198 | 
199 |   <gazebo reference="gripper_right">
200 |     <material>Gazebo/White</material>
201 |     <turnGravityOff>false</turnGravityOff>
202 |   </gazebo>
203 | 
204 |   <!-- RIGHT FINGER -->
205 | 
206 |   <joint name="guide_joint_finger_right" type="fixed">
207 |      <origin xyz="0 0 0.023" rpy="0 0 0" />
208 |      <parent link="gripper_right"/>
209 |      <child link="finger_right" />
210 |      <dynamics friction="100" damping="100" />
211 |      <axis xyz="1 0 0"/>
212 |      <limit effort="100" velocity="100"/>
213 |   </joint>
214 | 
215 |   <link name="finger_right">
216 |       <inertial>
217 |           <mass value="0.1" />
218 |           <origin xyz="0 0 0" />
219 |       <inertia ixx="1.0" ixy="0.0" ixz="0.0"
220 |                iyy="1.0" iyz="0.0" 
221 |                izz="1.0" />
222 |       </inertial>
223 | 
224 |       <visual>
225 |           <origin xyz="0.0 0 0" rpy="0 0 0" />
226 |           <geometry>
227 |              <mesh filename="package://wsg_50_simulation/meshes/WSG-FMF.stl" scale="0.001 0.001 0.001"/>
228 |           </geometry>
229 |           <material name="black">
230 | 		<color rgba="0 0 0 1"/>
231 |       	  </material>
232 |       </visual>
233 | 
234 |       <collision>
235 |           <origin xyz="0 0 0" rpy="0 0 0" />
236 |           <geometry>
237 |              <mesh filename="package://wsg_50_simulation/meshes/WSG-FMF.stl" scale="0.001 0.001 0.001"/>
238 |          </geometry>
239 |      </collision>
240 |   </link>
241 | 
242 |   <gazebo reference="finger_right">
243 |     <material>Gazebo/White</material>
244 |     <turnGravityOff>false</turnGravityOff>
245 |   </gazebo>
246 | 
247 |   <!-- Gazebo controllers -->
248 |   <!--gazebo>
249 |      <controller:gazebo_ros_time name="gazebo_ros_time" plugin="libgazebo_ros_time.so">
250 |        <alwaysOn>true</alwaysOn>
251 |        <updateRate>1000.0</updateRate>
252 |        <interface:audio name="dummy_gazebo_ros_time_iface_should_not_be_here"/>
253 |      </controller:gazebo_ros_time-->
254 | 
255 |   <!-- PR2_ACTARRAY -->
256 |   <gazebo>
257 |      <controller:gazebo_ros_controller_manager name="gazebo_ros_controller_manager" plugin="libgazebo_ros_controller_manager.so">
258 |        <alwaysOn>true</alwaysOn>
259 |        <updateRate>1000.0</updateRate>
260 |        <interface:audio name="gazebo_ros_controller_manager_dummy_iface" />
261 |      </controller:gazebo_ros_controller_manager>
262 |   </gazebo>
263 | 
264 | 
265 | </robot>
266 | 
267 | 
268 | 


--------------------------------------------------------------------------------
/wsg_50_simulation/urdf/wsg_50.urdf.xacro:
--------------------------------------------------------------------------------
  1 | <?xml version="1.0"?>
  2 | <robot  xmlns:sensor="http://playerstage.sourceforge.net/gazebo/xmlschema/#sensor"
  3 | 	xmlns:controller="http://playerstage.sourceforge.net/gazebo/xmlschema/#controller"
  4 | 	xmlns:interface="http://playerstage.sourceforge.net/gazebo/xmlschema/#interface"
  5 |         xmlns:xacro="http://ros.org/wiki/xacro"
  6 | 	name="wsg_50">
  7 | 
  8 | <!-- finger_length: Length of finger from gripper base. (was 0.023) -->
  9 | <!-- finger_tip:    Offset of finger tip along fingers' motion direction.
 10 |                     Use e.g. for single contact point tip on finger. (default 0) -->
 11 | <xacro:macro name="wsg_50_xacro" params="name parent finger_length finger_tip *origin">
 12 | 
 13 |  <joint name="${name}_anterior_gripper_joint" type="fixed">
 14 |     <xacro:insert_block name="origin"/>
 15 |     <parent link="${parent}"/>
 16 |     <child link="${name}_base_link"/>
 17 |  </joint>
 18 | 
 19 | <!-- BASE LINK -->
 20 |  <link name="${name}_base_link">
 21 |       <inertial>
 22 |           <mass value="0.25" />
 23 |           <origin xyz="0 0 0" />
 24 |      	  <!--inertia ixx="1.0" ixy="0.0" ixz="0.0"
 25 |                iyy="100.0" iyz="0.0" 
 26 |                izz="1.0" /-->
 27 |       	  <inertia  ixx="1.0" ixy="0.0"  ixz="0.0"  iyy="1.0"  iyz="0.0"  izz="1.0" />
 28 |       </inertial>
 29 | 
 30 |       <visual>
 31 |           <origin xyz="0.0 0 0" rpy="0 0 0" />
 32 |           <geometry>
 33 |              <mesh filename="package://wsg_50_simulation/meshes/WSG50_110.stl" scale="1 1 1"/>
 34 | 	     <!--box size="0.146 0.05 0.0735"/-->
 35 |           </geometry>
 36 |           <material name="grey">
 37 | 		<color rgba="0.5 0.5 0.5 1"/>
 38 |       	  </material>
 39 |       </visual>
 40 | 
 41 |       <collision>
 42 |           <origin xyz="0.0 0 0" rpy="0 0 0" />
 43 |           <geometry>
 44 | 	     <!--box size="0.146 0.05 0.0735"/-->
 45 |              <mesh filename="package://wsg_50_simulation/meshes/WSG50_110.stl" scale="1 1 1"/>
 46 |          </geometry>
 47 |      </collision>
 48 |   </link>
 49 | 
 50 |   <gazebo reference="${name}_base_link">
 51 |     <material>Gazebo/Grey</material>
 52 |     <turnGravityOff>false</turnGravityOff>
 53 |   </gazebo>
 54 | 
 55 |   <joint name="${name}_center_joint" type="fixed">
 56 |      <origin xyz="0 0 ${finger_length}" rpy="0 0 0" />      <!--origin xyz="-0.0067 0 0.049" rpy="0 0 0" /-->
 57 |      <parent link="${name}_base_link"/>
 58 |      <child link="${name}_center" />
 59 |   </joint>
 60 | 
 61 | 
 62 |   <link name="${name}_center">
 63 |   </link>
 64 | 
 65 |   <!-- GRIPPER LEFT -->
 66 |   <joint name="${name}_finger_left_joint" type="prismatic">
 67 |      <limit lower="-0.055" upper="-0.0027" effort="1.0" velocity="1.0"/>
 68 |      <origin xyz="0 0 0" rpy="0 0 0" />      <!--origin xyz="-0.0067 0 0.049" rpy="0 0 0" /-->
 69 |      <parent link="${name}_base_link"/>
 70 |      <child link="${name}_gripper_left" />
 71 |      <dynamics friction="100" damping="100" />
 72 |      <axis xyz="1 0 0"/>
 73 |      <limit effort="100" velocity="100"/>
 74 |   </joint>
 75 | 
 76 |   <transmission name="${name}_base_trans_left" type="pr2_mechanism_model/SimpleTransmission">
 77 |      <actuator name="${name}_base_motor_left" />
 78 |      <joint name="${name}_base_joint_gripper_left" />
 79 |      <mechanicalReduction>1</mechanicalReduction>
 80 |      <motorTorqueConstant>1</motorTorqueConstant>
 81 |   </transmission>
 82 | 
 83 |   <link name="${name}_gripper_left">
 84 |       <inertial>
 85 |           <mass value="0.1" />
 86 |           <origin xyz="0 0 0" />
 87 |       <inertia ixx="1.0" ixy="0.0" ixz="0.0"
 88 |                iyy="1.0" iyz="0.0" 
 89 |                izz="1.0" />
 90 |       </inertial>
 91 | 
 92 |       <visual>
 93 |           <origin xyz="0.0 0 0" rpy="0 0 0" />
 94 |           <geometry>
 95 |              <mesh filename="package://wsg_50_simulation/meshes/GUIDE_WSG50_110.stl" scale="0.001 0.001 0.001"/>
 96 | 	     <!--box size="0.0175 0.03 0.024"/-->
 97 |           </geometry>
 98 |           <material name="black">
 99 | 		<color rgba="0 0 0 0"/>
100 |       	  </material>
101 |       </visual>
102 | 
103 |       <collision>
104 |           <origin xyz="0 0 0" rpy="0 0 0" />
105 |           <geometry>
106 | 	     <!--box size="0.0175 0.03 0.024"/-->
107 |              <mesh filename="package://wsg_50_simulation/meshes/GUIDE_WSG50_110.stl" scale="0.001 0.001 0.001"/>
108 |          </geometry>
109 |      </collision>
110 |      <contact_coefficients kd="1.0" kp="1000.0" mu="0"/>
111 |   </link>
112 | 
113 |   <gazebo reference="${name}_gripper_left">
114 |     <material>Gazebo/Blue</material>
115 |     <turnGravityOff>false</turnGravityOff>
116 |   </gazebo>
117 | 
118 | 
119 |   <!-- LEFT FINGER -->
120 |   <joint name="${name}_guide_joint_finger_left" type="fixed">
121 |      <origin xyz="${finger_tip} 0 ${finger_length}" rpy="0 0 0" />
122 |      <parent link="${name}_gripper_left"/>
123 |      <child link="${name}_finger_left" />
124 |      <dynamics friction="100" damping="100" />
125 |      <axis xyz="1 0 0"/>
126 |      <limit effort="100" velocity="100"/>
127 |   </joint>
128 | 
129 |   <link name="${name}_finger_left">
130 |       <inertial>
131 |           <mass value="0.1" />
132 |           <origin xyz="0 0 0" />
133 |       <inertia ixx="1.0" ixy="0.0" ixz="0.0"
134 |                iyy="1.0" iyz="0.0" 
135 |                izz="1.0" />
136 |       </inertial>
137 | 
138 |       <visual>
139 |           <origin xyz="0.0 0 0" rpy="0 0 0" />
140 |           <geometry>
141 |              <mesh filename="package://wsg_50_simulation/meshes/WSG-FMF.stl" scale="0.001 0.001 0.001"/>
142 |           </geometry>
143 |           <material name="black">
144 | 		<color rgba="0 0 0 0"/>
145 |       	  </material>
146 |       </visual>
147 | 
148 |       <collision>
149 |           <origin xyz="0 0 0" rpy="0 0 0" />
150 |           <geometry>
151 |              <mesh filename="package://wsg_50_simulation/meshes/WSG-FMF.stl" scale="0.001 0.001 0.001"/>
152 |          </geometry>
153 |      </collision>
154 |      <contact_coefficients kd="1.0" kp="1000.0" mu="0"/>
155 |   </link>
156 | 
157 |   <gazebo reference="${name}_finger_left">
158 |     <material>Gazebo/Blue</material>
159 |     <turnGravityOff>false</turnGravityOff>
160 |   </gazebo>
161 | 
162 | 
163 |   <!-- GRIPPER RIGHT -->
164 | 
165 |   <joint name="${name}_finger_right_joint" type="prismatic">
166 |      <limit lower="0.0027" upper="0.055" effort="1.0" velocity="1.0"/>
167 |      <origin xyz="0 0 0" rpy="0 0 3.14159" />
168 |      <parent link="${name}_base_link"/>
169 |      <child link="${name}_gripper_right" />
170 |      <axis xyz="-1 0 0"/>
171 |      <dynamics friction="100" damping="100" />
172 |      <limit effort="100" velocity="100"/>
173 |   </joint>
174 | 
175 |   <transmission name="${name}_base_trans_right" type="pr2_mechanism_model/SimpleTransmission">
176 |      <actuator name="${name}_base_motor_right" />
177 |      <joint name="${name}_base_joint_gripper_right" />
178 |      <mechanicalReduction>1</mechanicalReduction>
179 |      <motorTorqueConstant>1</motorTorqueConstant>
180 |   </transmission>
181 | 	
182 |     <link name="${name}_gripper_right">
183 |       <inertial>
184 |           <mass value="0.1" />
185 |           <origin xyz="0 0 0" />
186 |       <inertia ixx="1.0" ixy="0.0" ixz="0.0"
187 |                iyy="1.0" iyz="0.0" 
188 |                izz="1.0" />
189 |       </inertial>
190 | 
191 |       <visual>
192 |           <origin xyz="0.0 0 0" rpy="0 0 0" />
193 |           <geometry>
194 |              <mesh filename="package://wsg_50_simulation/meshes/GUIDE_WSG50_110.stl" scale="0.001 0.001 0.001"/>
195 | 	     <!--box size="0.0175 0.03 0.024"/-->
196 |           </geometry>
197 |           <material name="black">
198 | 		<color rgba="0 0 0 0"/>
199 |       	  </material>
200 |       </visual>
201 | 
202 |       <collision>
203 |           <origin xyz="0 0 0" rpy="0 0 0" />
204 |           <geometry>
205 | 	     <!--box size="0.0175 0.03 0.024"/-->
206 |              <mesh filename="package://wsg_50_simulation/meshes/GUIDE_WSG50_110.stl" scale="0.001 0.001 0.001"/>
207 |          </geometry>
208 |      </collision>
209 |      <contact_coefficients kd="1.0" kp="1000.0" mu="0"/>
210 |   </link>
211 | 
212 |   <gazebo reference="gripper_right">
213 |     <material>Gazebo/White</material>
214 |     <turnGravityOff>false</turnGravityOff>
215 |   </gazebo>
216 | 
217 |   <!-- RIGHT FINGER -->
218 | 
219 |   <joint name="${name}_guide_joint_finger_right" type="fixed">
220 |      <origin xyz="${finger_tip} 0 ${finger_length}" rpy="0 0 0" />
221 |      <parent link="${name}_gripper_right"/>
222 |      <child link="${name}_finger_right" />
223 |      <dynamics friction="100" damping="100" />
224 |      <axis xyz="1 0 0"/>
225 |      <limit effort="100" velocity="100"/>
226 |   </joint>
227 | 
228 |   <link name="${name}_finger_right">
229 |       <inertial>
230 |           <mass value="0.1" />
231 |           <origin xyz="0 0 0" />
232 |       <inertia ixx="1.0" ixy="0.0" ixz="0.0"
233 |                iyy="1.0" iyz="0.0" 
234 |                izz="1.0" />
235 |       </inertial>
236 | 
237 |       <visual>
238 |           <origin xyz="0.0 0 0" rpy="0 0 0" />
239 |           <geometry>
240 |              <mesh filename="package://wsg_50_simulation/meshes/WSG-FMF.stl" scale="0.001 0.001 0.001"/>
241 |           </geometry>
242 |           <material name="black">
243 | 		<color rgba="0 0 0 0"/>
244 |       	  </material>
245 |       </visual>
246 | 
247 |       <collision>
248 |           <origin xyz="0 0 0" rpy="0 0 0" />
249 |           <geometry>
250 |              <mesh filename="package://wsg_50_simulation/meshes/WSG-FMF.stl" scale="0.001 0.001 0.001"/>
251 |          </geometry>
252 |      </collision>
253 |      <contact_coefficients kd="1.0" kp="1000.0" mu="0"/>
254 |   </link>
255 | 
256 |   <gazebo reference="${name}_finger_right">
257 |     <material>Gazebo/White</material>
258 |     <turnGravityOff>false</turnGravityOff>
259 |   </gazebo>
260 | 
261 | </xacro:macro>
262 | 
263 | </robot>
264 | 
265 | 


--------------------------------------------------------------------------------
/wsg_50_simulation/urdf/wsg_50_dependent_joints.yaml:
--------------------------------------------------------------------------------
1 | dependent_joints:
2 |   base_joint_gripper_left: {parent: base_joint_gripper_right, factor: -1 }
3 | 


--------------------------------------------------------------------------------
/wsg_50_simulation/wsg_50_gl.yaml:
--------------------------------------------------------------------------------
1 | wsg_50_gl:
2 |   type: robot_mechanism_controllers/JointPositionController
3 |   joint: base_joint_gripper_left
4 |   pid:
5 |     p: 100.0
6 |     d: 10.0
7 | 
8 | 


--------------------------------------------------------------------------------
/wsg_50_simulation/wsg_50_gr.yaml:
--------------------------------------------------------------------------------
1 | wsg_50_gr:
2 |   type: robot_mechanism_controllers/JointPositionController
3 |   joint: base_joint_gripper_right
4 |   pid:
5 |     p: 100.0
6 |     d: 10.0
7 | 
8 | 


--------------------------------------------------------------------------------