diff --git a/training/supplements/src/industrial_core/.gitignore b/training/supplements/src/industrial_core/.gitignore deleted file mode 100644 index 07f6c40d..00000000 --- a/training/supplements/src/industrial_core/.gitignore +++ /dev/null @@ -1,27 +0,0 @@ -# Compiled Object files -*.slo -*.lo -*.o - -# Compiled Dynamic libraries -*.so -*.dylib - -# Compiled Static libraries -*.lai -*.la -*.a - -# Left over subversion files -*.svn - -# Temp files -*~ - -# Other ROS stuff -CATKIN_IGNORE* - -# Eclipse stuff -.pydevproject -.project -.cproject diff --git a/training/supplements/src/industrial_core/README.md b/training/supplements/src/industrial_core/README.md deleted file mode 100644 index 4b628df0..00000000 --- a/training/supplements/src/industrial_core/README.md +++ /dev/null @@ -1,11 +0,0 @@ -# Industrial Core -[![Build Status](http://jenkins.ros.org/job/devel-hydro-industrial_core/badge/icon)](http://jenkins.ros.org/job/devel-hydro-industrial_core/)
-[ROS-Industrial][] core meta-package. See the [ROS wiki][] page for more information. - -## Contents - -This repo holds source code for all versions > groovy. For those versions <= groovy see: [SVN repo][] - -[ROS-Industrial]: http://www.ros.org/wiki/Industrial -[ROS wiki]: http://ros.org/wiki/industrial_core -[SVN repo]: https://code.google.com/p/swri-ros-pkg/source/browse diff --git a/training/supplements/src/industrial_core/industrial_core/CHANGELOG.rst b/training/supplements/src/industrial_core/industrial_core/CHANGELOG.rst deleted file mode 100644 index 57abb298..00000000 --- a/training/supplements/src/industrial_core/industrial_core/CHANGELOG.rst +++ /dev/null @@ -1,15 +0,0 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package industrial_core -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -0.3.3 (2014-01-13) ------------------- -* Fixed build issue due simple message library linking -0.3.2 (2014-01-10) ------------------- -* Removed header from industrial_utils/utils.h (not required) -0.3.1 (2014-01-09) ------------------- -* Added industrial trajectory filters to core meta-package -* Converted to catkin -* Contributors: Shaun Edwards, gavanderhoorn diff --git a/training/supplements/src/industrial_core/industrial_core/CMakeLists.txt b/training/supplements/src/industrial_core/industrial_core/CMakeLists.txt deleted file mode 100644 index 7c8b2ed9..00000000 --- a/training/supplements/src/industrial_core/industrial_core/CMakeLists.txt +++ /dev/null @@ -1,4 +0,0 @@ -cmake_minimum_required(VERSION 2.8.3) -project(industrial_core) -find_package(catkin REQUIRED) -catkin_metapackage() diff --git a/training/supplements/src/industrial_core/industrial_core/package.xml b/training/supplements/src/industrial_core/industrial_core/package.xml deleted file mode 100644 index 07bfb9e1..00000000 --- a/training/supplements/src/industrial_core/industrial_core/package.xml +++ /dev/null @@ -1,23 +0,0 @@ - - industrial_core - 0.3.3 - ROS-Industrial core stack contains packages and libraries for supporing industrial systems - - Shaun Edwards - BSD - http://ros.org/wiki/industrial_core - Shaun Edwards - - catkin - simple_message - industrial_msgs - industrial_robot_client - industrial_robot_simulator - industrial_deprecated - industrial_utils - industrial_trajectory_filters - - - - - diff --git a/training/supplements/src/industrial_core/industrial_deprecated/CHANGELOG.rst b/training/supplements/src/industrial_core/industrial_deprecated/CHANGELOG.rst deleted file mode 100644 index f9c6b2a4..00000000 --- a/training/supplements/src/industrial_core/industrial_deprecated/CHANGELOG.rst +++ /dev/null @@ -1,18 +0,0 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package industrial_deprecated -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -0.3.3 (2014-01-13) ------------------- -* No change -0.3.2 (2014-01-10) ------------------- -* No change - -0.3.1 (2014-01-09) ------------------- -* Purged deprecated packages -* Remove obsolete export tags. Fix `#43 `_. -* Corrected spelling of industrial_deprecated package (oops). Updated package dependencies. -* Converted to catkin -* Contributors: Shaun Edwards, gavanderhoorn, jrgnicho diff --git a/training/supplements/src/industrial_core/industrial_deprecated/CMakeLists.txt b/training/supplements/src/industrial_core/industrial_deprecated/CMakeLists.txt deleted file mode 100644 index 1804e2d6..00000000 --- a/training/supplements/src/industrial_core/industrial_deprecated/CMakeLists.txt +++ /dev/null @@ -1,10 +0,0 @@ -cmake_minimum_required(VERSION 2.8.3) - -project(industrial_deprecated) - -find_package(catkin REQUIRED COMPONENTS) - -catkin_package( - CATKIN_DEPENDS -) - diff --git a/training/supplements/src/industrial_core/industrial_deprecated/mainpage.dox b/training/supplements/src/industrial_core/industrial_deprecated/mainpage.dox deleted file mode 100644 index dc3ec8ff..00000000 --- a/training/supplements/src/industrial_core/industrial_deprecated/mainpage.dox +++ /dev/null @@ -1,14 +0,0 @@ -/** -\mainpage -\htmlinclude manifest.html - -\b industrial_deprecated - - - ---> - - -*/ diff --git a/training/supplements/src/industrial_core/industrial_deprecated/package.xml b/training/supplements/src/industrial_core/industrial_deprecated/package.xml deleted file mode 100644 index a892801a..00000000 --- a/training/supplements/src/industrial_core/industrial_deprecated/package.xml +++ /dev/null @@ -1,16 +0,0 @@ - - industrial_deprecated - 0.3.3 - - The Industrial deprecated package contains nodes, launch files, etc... that are slated for - deprecation. This package is the last place something will end up before being deleted. - If you are missing a package/node and find it's contents here, then you should consider - a replacement. - - Shaun Edwards - BSD - http://ros.org/wiki/industrial_deprecated - Shaun M. Edwards - - catkin - diff --git a/training/supplements/src/industrial_core/industrial_msgs/CHANGELOG.rst b/training/supplements/src/industrial_core/industrial_msgs/CHANGELOG.rst deleted file mode 100644 index 813e142b..00000000 --- a/training/supplements/src/industrial_core/industrial_msgs/CHANGELOG.rst +++ /dev/null @@ -1,17 +0,0 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package industrial_msgs -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -0.3.3 (2014-01-13) ------------------- -* No change -0.3.2 (2014-01-10) ------------------- -* No change - -0.3.1 (2014-01-09) ------------------- -* Remove obsolete export tags. Fix `#43 `_. -* add joint_path_command service to Industrial Robot Client -* Converted to catkin -* Contributors: JeremyZoss, Shaun Edwards, gavanderhoorn, shaun-edwards diff --git a/training/supplements/src/industrial_core/industrial_msgs/CMakeLists.txt b/training/supplements/src/industrial_core/industrial_msgs/CMakeLists.txt deleted file mode 100644 index 9c6523ee..00000000 --- a/training/supplements/src/industrial_core/industrial_msgs/CMakeLists.txt +++ /dev/null @@ -1,31 +0,0 @@ -cmake_minimum_required(VERSION 2.8.3) - -project(industrial_msgs) - -find_package(catkin REQUIRED COMPONENTS std_msgs trajectory_msgs genmsg message_generation) - -add_message_files( - FILES - DebugLevel.msg - DeviceInfo.msg - RobotMode.msg - RobotStatus.msg - ServiceReturnCode.msg - TriState.msg) - -add_service_files( - FILES - CmdJointTrajectory.srv - GetRobotInfo.srv - SetDrivePower.srv - SetRemoteLoggerLevel.srv - StartMotion.srv - StopMotion.srv) - -generate_messages( - DEPENDENCIES trajectory_msgs std_msgs -) - -catkin_package( - CATKIN_DEPENDS message_runtime std_msgs trajectory_msgs genmsg -) \ No newline at end of file diff --git a/training/supplements/src/industrial_core/industrial_msgs/mainpage.dox b/training/supplements/src/industrial_core/industrial_msgs/mainpage.dox deleted file mode 100644 index ee13539c..00000000 --- a/training/supplements/src/industrial_core/industrial_msgs/mainpage.dox +++ /dev/null @@ -1,14 +0,0 @@ -/** -\mainpage -\htmlinclude manifest.html - -\b The industrial message package containes industrial specific messages definitions. This package is part of the ROS-Industrial program. - - - ---> - - -*/ diff --git a/training/supplements/src/industrial_core/industrial_msgs/msg/DebugLevel.msg b/training/supplements/src/industrial_core/industrial_msgs/msg/DebugLevel.msg deleted file mode 100644 index 346bd7f5..00000000 --- a/training/supplements/src/industrial_core/industrial_msgs/msg/DebugLevel.msg +++ /dev/null @@ -1,11 +0,0 @@ -# Debug level message enumeration. This may replicate some functionality that -# alreay exists in the ROS logger. -# TODO: Get more information on the ROS Logger. -uint8 val - -uint8 DEBUG = 5 -uint8 INFO = 4 -uint8 WARN = 3 -uint8 ERROR = 2 -uint8 FATAL = 1 -uint8 NONE = 0 diff --git a/training/supplements/src/industrial_core/industrial_msgs/msg/DeviceInfo.msg b/training/supplements/src/industrial_core/industrial_msgs/msg/DeviceInfo.msg deleted file mode 100644 index 2da2025d..00000000 --- a/training/supplements/src/industrial_core/industrial_msgs/msg/DeviceInfo.msg +++ /dev/null @@ -1,11 +0,0 @@ -# Device info captures device agnostic information about a piece of hardware. -# This message is meant as a generic as possible. Items that don't apply should -# be left blank. This message is not meant to replace diagnostic messages, but -# rather provide a standard service message that can be used to populate standard -# components (like a GUI for example) - -string model -string serial_number -string hw_version -string sw_version -string address diff --git a/training/supplements/src/industrial_core/industrial_msgs/msg/RobotMode.msg b/training/supplements/src/industrial_core/industrial_msgs/msg/RobotMode.msg deleted file mode 100644 index fdcb79f6..00000000 --- a/training/supplements/src/industrial_core/industrial_msgs/msg/RobotMode.msg +++ /dev/null @@ -1,10 +0,0 @@ -# The Robot mode message encapsulates the mode/teach state of the robot -# Typically this is controlled by the pendant key switch, but not always - -int8 val - -# enumerated values -int8 UNKNOWN=-1 # Unknown or unavailable -int8 MANUAL=1 # Teach OR manual mode -int8 AUTO=2 # Automatic mode - diff --git a/training/supplements/src/industrial_core/industrial_msgs/msg/RobotStatus.msg b/training/supplements/src/industrial_core/industrial_msgs/msg/RobotStatus.msg deleted file mode 100644 index b26a31b5..00000000 --- a/training/supplements/src/industrial_core/industrial_msgs/msg/RobotStatus.msg +++ /dev/null @@ -1,32 +0,0 @@ -# The RobotStatus message contains low level status information -# that is specific to an industrial robot controller - -# The header frame ID is not used -Header header - -# The robot mode captures the operating mode of the robot. When in -# manual, remote motion is not possible. -industrial_msgs/RobotMode mode - -# Estop status: True if robot is e-stopped. The drives are disabled -# and motion is not possible. The e-stop condition must be acknowledged -# and cleared before any motion can begin. -industrial_msgs/TriState e_stopped - -# Drive power status: True if drives are powered. Motion commands will -# automatically enable the drives if required. Drive power is not requred -# for possible motion -industrial_msgs/TriState drives_powered - -# Motion enabled: Ture if robot motion is possible. -industrial_msgs/TriState motion_possible - -# Motion status: True if robot is in motion, otherwise false -industrial_msgs/TriState in_motion - -# Error status: True if there is an error condition on the robot. Motion may -# or may not be affected (see motion_possible) -industrial_msgs/TriState in_error - -# Error code: Vendor specific error code (non zero indicates error) -int32 error_code diff --git a/training/supplements/src/industrial_core/industrial_msgs/msg/ServiceReturnCode.msg b/training/supplements/src/industrial_core/industrial_msgs/msg/ServiceReturnCode.msg deleted file mode 100644 index 25782658..00000000 --- a/training/supplements/src/industrial_core/industrial_msgs/msg/ServiceReturnCode.msg +++ /dev/null @@ -1,8 +0,0 @@ -# Service return codes for simple requests. All ROS-Industrial service -# replies are required to have a return code indicating success or failure -# Specific return codes for different failure should be negative. -int8 val - -int8 SUCCESS = 1 -int8 FAILURE = -1 - diff --git a/training/supplements/src/industrial_core/industrial_msgs/msg/TriState.msg b/training/supplements/src/industrial_core/industrial_msgs/msg/TriState.msg deleted file mode 100644 index c5766265..00000000 --- a/training/supplements/src/industrial_core/industrial_msgs/msg/TriState.msg +++ /dev/null @@ -1,23 +0,0 @@ -# The tri-state captures boolean values with the additional state of unknown - -int8 val - -# enumerated values - -# Unknown or unavailable -int8 UNKNOWN=-1 - -# High state -int8 TRUE=1 -int8 ON=1 -int8 ENABLED=1 -int8 HIGH=1 -int8 CLOSED=1 - -# Low state -int8 FALSE=0 -int8 OFF=0 -int8 DISABLED=0 -int8 LOW=0 -int8 OPEN=0 - diff --git a/training/supplements/src/industrial_core/industrial_msgs/package.xml b/training/supplements/src/industrial_core/industrial_msgs/package.xml deleted file mode 100644 index ade54cbf..00000000 --- a/training/supplements/src/industrial_core/industrial_msgs/package.xml +++ /dev/null @@ -1,22 +0,0 @@ - - industrial_msgs - 0.3.3 - - The industrial message package containes industrial specific messages - definitions. This package is part of the ROS-Industrial program. - - Shaun Edwards - BSD - http://ros.org/wiki/industrial_msg - Shaun M. Edwards - - catkin - std_msgs - trajectory_msgs - genmsg - message_generation - std_msgs - trajectory_msgs - genmsg - message_runtime - diff --git a/training/supplements/src/industrial_core/industrial_msgs/srv/CmdJointTrajectory.srv b/training/supplements/src/industrial_core/industrial_msgs/srv/CmdJointTrajectory.srv deleted file mode 100644 index 2fbab979..00000000 --- a/training/supplements/src/industrial_core/industrial_msgs/srv/CmdJointTrajectory.srv +++ /dev/null @@ -1,9 +0,0 @@ -# Send a JointTrajectory command to the robot. -# - duplicates functionality of the joint_path_command topic -# - provides a response-code to verify command was received -# - returns when trajectory is sent to robot, not when motion completed -# - return code may NOT indicate successful transfer to robot - -trajectory_msgs/JointTrajectory trajectory ---- -industrial_msgs/ServiceReturnCode code diff --git a/training/supplements/src/industrial_core/industrial_msgs/srv/GetRobotInfo.srv b/training/supplements/src/industrial_core/industrial_msgs/srv/GetRobotInfo.srv deleted file mode 100644 index 748e1ec6..00000000 --- a/training/supplements/src/industrial_core/industrial_msgs/srv/GetRobotInfo.srv +++ /dev/null @@ -1,7 +0,0 @@ -# The Get Robot Info service returns vendor specific information about -# the robot(s) connected by the driver - ---- -industrial_msgs/DeviceInfo controller -industrial_msgs/DeviceInfo[] robots -industrial_msgs/ServiceReturnCode code diff --git a/training/supplements/src/industrial_core/industrial_msgs/srv/SetDrivePower.srv b/training/supplements/src/industrial_core/industrial_msgs/srv/SetDrivePower.srv deleted file mode 100644 index 79b2088c..00000000 --- a/training/supplements/src/industrial_core/industrial_msgs/srv/SetDrivePower.srv +++ /dev/null @@ -1,7 +0,0 @@ -# Direct method of turning drive power on and off -# NOTE: Motion commands will automatically to this if -# drive power is not on and a motion command is received. - -bool drive_power ---- -industrial_msgs/ServiceReturnCode code diff --git a/training/supplements/src/industrial_core/industrial_msgs/srv/SetRemoteLoggerLevel.srv b/training/supplements/src/industrial_core/industrial_msgs/srv/SetRemoteLoggerLevel.srv deleted file mode 100644 index 9515e21f..00000000 --- a/training/supplements/src/industrial_core/industrial_msgs/srv/SetRemoteLoggerLevel.srv +++ /dev/null @@ -1,8 +0,0 @@ -# Sets logging level for a remote device that is attached to -# ROS via some communications link. This service is meant to -# set the log level on the device directly so that the comms -# link is not overloaded with messages. - -industrial_msgs/DebugLevel level ---- -industrial_msgs/ServiceReturnCode code diff --git a/training/supplements/src/industrial_core/industrial_msgs/srv/StartMotion.srv b/training/supplements/src/industrial_core/industrial_msgs/srv/StartMotion.srv deleted file mode 100644 index 8c5b4d45..00000000 --- a/training/supplements/src/industrial_core/industrial_msgs/srv/StartMotion.srv +++ /dev/null @@ -1,3 +0,0 @@ -# Resume current robot motion (e.g. after stop_motion or robot fault) ---- -industrial_msgs/ServiceReturnCode code diff --git a/training/supplements/src/industrial_core/industrial_msgs/srv/StopMotion.srv b/training/supplements/src/industrial_core/industrial_msgs/srv/StopMotion.srv deleted file mode 100644 index f7546ed3..00000000 --- a/training/supplements/src/industrial_core/industrial_msgs/srv/StopMotion.srv +++ /dev/null @@ -1,5 +0,0 @@ -# Stops current robot motion. Motion resumed by using start_motion service -# or by sending a new motion command - ---- -industrial_msgs/ServiceReturnCode code diff --git a/training/supplements/src/industrial_core/industrial_robot_client/CHANGELOG.rst b/training/supplements/src/industrial_core/industrial_robot_client/CHANGELOG.rst deleted file mode 100644 index 5a4c1cbd..00000000 --- a/training/supplements/src/industrial_core/industrial_robot_client/CHANGELOG.rst +++ /dev/null @@ -1,19 +0,0 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package industrial_robot_client -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -0.3.3 (2014-01-13) ------------------- -* Fixed build issue due simple message library linking -* Contributors: gavanderhoorn - -0.3.2 (2014-01-10) ------------------- -* Removed header from industrial_utils/utils.h (not required) - -0.3.1 (2014-01-09) ------------------- -* Remove obsolete export tags. Fix `#43 `_. -* Removed library export from catkin macro. Packages that depend on these must declare library dependencies explicitly (by name) -* Converted to catkin -* Contributors: JeremyZoss, Shaun Edwards, gavanderhoorn diff --git a/training/supplements/src/industrial_core/industrial_robot_client/CMakeLists.txt b/training/supplements/src/industrial_core/industrial_robot_client/CMakeLists.txt deleted file mode 100644 index 95f6298d..00000000 --- a/training/supplements/src/industrial_core/industrial_robot_client/CMakeLists.txt +++ /dev/null @@ -1,178 +0,0 @@ -cmake_minimum_required(VERSION 2.8.3) - -project(industrial_robot_client) - -find_package(catkin REQUIRED COMPONENTS roscpp std_msgs sensor_msgs - control_msgs trajectory_msgs simple_message actionlib_msgs actionlib - urdf industrial_msgs industrial_utils) - -find_package(Boost REQUIRED COMPONENTS system thread) - -# The definition is copied from the CMakeList for the simple_message package. -add_definitions(-DROS=1) #build using ROS libraries -add_definitions(-DLINUXSOCKETS=1) #build using LINUX SOCKETS libraries - -set(SRC_FILES src/joint_relay_handler.cpp - src/robot_status_relay_handler.cpp - src/joint_trajectory_downloader.cpp - src/joint_trajectory_streamer.cpp - src/joint_trajectory_interface.cpp - src/robot_state_interface.cpp - src/utils.cpp) - -# NOTE: The libraries generated this package are not included in the catkin_package -# macro because libraries must be explicitly linked in projects that depend on this -# package. If this is not done (and these libraries were exported), then multiple -# library definitions (normal - industrial_robot_client and byteswapped. -# industrial_robot_client_bswap) are both included (this is bad). -catkin_package( - CATKIN_DEPENDS roscpp std_msgs sensor_msgs control_msgs trajectory_msgs - simple_message actionlib_msgs actionlib urdf industrial_msgs - industrial_utils - INCLUDE_DIRS include - LIBRARIES ${PROJECT_NAME}_dummy - CFG_EXTRAS issue46_workaround.cmake -) - - - -include_directories(include - ${catkin_INCLUDE_DIRS} -) - - -# generate dummy library (we export it in catkin_package(..)), to force catkin -# to set up LIBRARY_DIRS properly. -# TODO: find out if LIBRARY_DIRS can be exported without dummy library target -add_custom_command( - OUTPUT ${CMAKE_CURRENT_BINARY_DIR}/${PROJECT_NAME}_dummy.cpp - COMMAND ${CMAKE_COMMAND} -E touch ${CMAKE_CURRENT_BINARY_DIR}/${PROJECT_NAME}_dummy.cpp) -add_library(${PROJECT_NAME}_dummy ${CMAKE_CURRENT_BINARY_DIR}/${PROJECT_NAME}_dummy.cpp) -# unfortunately this will have to be installed, but the linker will remove it -# from the library dependencies of dependent targets, as it contains no symbols -# that can be imported from it. -install(TARGETS ${PROJECT_NAME}_dummy DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) - - -# This library depends on the simple_message library, which is available -# in two formats to support endian differences between robot & PC. -# -# As in simple_message, two libraries are created: one that does direct -# passthrough of binary numeric data to the robot controller, and one that -# uses byte-swapping to account for endian mis-matches. -# -# Due to catkin dependency limitations, higher-level code must -# explicitly link to the desired industrial_robot_client library: -# target_link_libraries(my_node industrial_robot_client) -# or -# target_link_libraries(my_node industrial_robot_client_bswap) -# - -add_library(industrial_robot_client ${SRC_FILES}) -target_link_libraries(industrial_robot_client simple_message) - -add_library(industrial_robot_client_bswap ${SRC_FILES}) -target_link_libraries(industrial_robot_client_bswap simple_message_bswap) - -# The following executables(nodes) are for applications where the robot -# controller and pc have the same byte order (i.e. byte swapping NOT -# required) - -add_executable(robot_state - src/generic_robot_state_node.cpp) -target_link_libraries(robot_state - industrial_robot_client - simple_message - ${catkin_LIBRARIES}) - -add_executable(motion_streaming_interface - src/generic_joint_streamer_node.cpp) -target_link_libraries(motion_streaming_interface - industrial_robot_client - simple_message - ${catkin_LIBRARIES}) - -add_executable(motion_download_interface - src/generic_joint_downloader_node.cpp) -target_link_libraries(motion_download_interface - industrial_robot_client - simple_message - ${catkin_LIBRARIES}) - -# The following executables(nodes) are for applications where the robot -# controller and pc have different same byte order (i.e. byte swapping IS -# required) - -add_executable(robot_state_bswap - src/generic_robot_state_node.cpp) -target_link_libraries(robot_state_bswap - industrial_robot_client_bswap - simple_message_bswap - ${catkin_LIBRARIES}) - -add_executable(motion_streaming_interface_bswap - src/generic_joint_streamer_node.cpp) -target_link_libraries(motion_streaming_interface_bswap - industrial_robot_client_bswap - simple_message_bswap - ${catkin_LIBRARIES}) - -add_executable(motion_download_interface_bswap - src/generic_joint_downloader_node.cpp) -target_link_libraries(motion_download_interface_bswap - industrial_robot_client_bswap - simple_message_bswap - ${catkin_LIBRARIES}) - -# The following executables(nodes) interface with the robot controller -# at a higher level so there is no need to create two versions (one with -# byte swapping, one without) - -add_executable(joint_trajectory_action - src/generic_joint_trajectory_action_node.cpp - src/joint_trajectory_action.cpp) -target_link_libraries(joint_trajectory_action - industrial_robot_client ${catkin_LIBRARIES}) -add_dependencies(joint_trajectory_action industrial_robot_client_gencpp) - -########## -## Test ## -########## -# Testing - Only performed on normal (non byte swapped library) -catkin_add_gtest(utest_robot_client test/utest.cpp) -target_link_libraries(utest_robot_client - industrial_robot_client - ${catkin_LIBRARIES}) - -# ROS launch testing -## ROS launch test should be enabled when launch parameters are supported, -## see details below: -## robot_interface_streaming.launch: 'robot_ip' -## robot_state_visualize.launch]: 'urdf_path' -## robot_interface_download.launch]: 'robot_ip' -##find_package(roslaunch) -##roslaunch_add_file_check(launch) - -install( - TARGETS industrial_robot_client industrial_robot_client_bswap - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) - -install(TARGETS - robot_state - robot_state_bswap - motion_streaming_interface - motion_download_interface - motion_streaming_interface_bswap - motion_download_interface_bswap - joint_trajectory_action - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) - -install( - DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}) - -foreach(dir config launch) - install(DIRECTORY ${dir}/ - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/${dir}) -endforeach(dir) - diff --git a/training/supplements/src/industrial_core/industrial_robot_client/cmake/issue46_workaround.cmake b/training/supplements/src/industrial_core/industrial_robot_client/cmake/issue46_workaround.cmake deleted file mode 100644 index cab929e4..00000000 --- a/training/supplements/src/industrial_core/industrial_robot_client/cmake/issue46_workaround.cmake +++ /dev/null @@ -1,10 +0,0 @@ -# -# Temporary workaround for issue ros-industrial/industrial_core#46. -# -message(STATUS "industrial_robot_client: work around for #46") -if (DEFINED industrial_robot_client_LIBRARY_DIRS) -else() - message(FATAL_ERROR "industrial_robot_client_LIBRARY_DIRS not set, " - "have you find_package()-ed it?") -endif() -link_directories(${industrial_robot_client_LIBRARY_DIRS}) diff --git a/training/supplements/src/industrial_core/industrial_robot_client/config/robot_state_visualize.rviz b/training/supplements/src/industrial_core/industrial_robot_client/config/robot_state_visualize.rviz deleted file mode 100644 index eca0bdce..00000000 --- a/training/supplements/src/industrial_core/industrial_robot_client/config/robot_state_visualize.rviz +++ /dev/null @@ -1,184 +0,0 @@ -Panels: - - Class: rviz/Displays - Help Height: 78 - Name: Displays - Property Tree Widget: - Expanded: - - /Global Options1 - - /Status1 - - /RobotModel1 - - /TF1 - Splitter Ratio: 0.5 - Tree Height: 787 - - Class: rviz/Selection - Name: Selection - - Class: rviz/Tool Properties - Expanded: - - /2D Pose Estimate1 - - /2D Nav Goal1 - Name: Tool Properties - Splitter Ratio: 0.588679 - - Class: rviz/Views - Expanded: - - /Current View1 - Name: Views - Splitter Ratio: 0.5 - - Class: rviz/Time - Experimental: false - Name: Time - SyncMode: 0 - SyncSource: "" -Visualization Manager: - Class: "" - Displays: - - Alpha: 0.5 - Cell Size: 1 - Class: rviz/Grid - Color: 160; 160; 164 - Enabled: true - Line Style: - Line Width: 0.03 - Value: Lines - Name: Grid - Normal Cell Count: 0 - Offset: - X: 0 - Y: 0 - Z: 0 - Plane: XY - Plane Cell Count: 10 - Reference Frame: - Value: true - - Alpha: 1 - Class: rviz/RobotModel - Collision Enabled: false - Enabled: true - Links: - base_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - link_1: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - link_2: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - link_3: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - link_4: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - link_5: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - link_6: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - Name: RobotModel - Robot Description: robot_description - TF Prefix: "" - Update Interval: 0 - Value: true - Visual Enabled: true - - Class: rviz/TF - Enabled: true - Frame Timeout: 15 - Frames: - /base_link: - Value: true - /link_1: - Value: true - /link_2: - Value: true - /link_3: - Value: true - /link_4: - Value: true - /link_5: - Value: true - /link_6: - Value: true - /tool0: - Value: true - All Enabled: true - Marker Scale: 1 - Name: TF - Show Arrows: true - Show Axes: true - Show Names: true - Tree: - /base_link: - /link_1: - /link_2: - /link_3: - /link_4: - /link_5: - /link_6: - /tool0: - {} - Update Interval: 0 - Value: true - Enabled: true - Global Options: - Background Color: 48; 48; 48 - Fixed Frame: /base_link - Name: root - Tools: - - Class: rviz/Interact - Hide Inactive Objects: true - - Class: rviz/MoveCamera - - Class: rviz/Select - - Class: rviz/Measure - - Class: rviz/SetInitialPose - Topic: /initialpose - - Class: rviz/SetGoal - Topic: /move_base_simple/goal - Value: true - Views: - Current: - Class: rviz/Orbit - Distance: 10 - Focal Point: - X: 0 - Y: 0 - Z: 0 - Name: Current View - Near Clip Distance: 0.01 - Pitch: 0.115398 - Target Frame: - Value: Orbit (rviz) - Yaw: 0.930399 - Saved: ~ -Window Geometry: - Displays: - collapsed: false - Height: 1000 - Hide Left Dock: false - Hide Right Dock: false - QMainWindow State: 000000ff00000000fd00000004000000000000013c000003a2fc0200000005fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000028000003a2000000dd00ffffff000000010000010f000003a2fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730100000028000003a2000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004a00000003efc0100000002fb0000000800540069006d00650000000000000004a0000002f700fffffffb0000000800540069006d00650100000000000004500000000000000000000002a9000003a200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 - Selection: - collapsed: false - Time: - collapsed: false - Tool Properties: - collapsed: false - Views: - collapsed: false - Width: 1280 - X: 60 - Y: 60 diff --git a/training/supplements/src/industrial_core/industrial_robot_client/config/robot_state_visualize.vcg b/training/supplements/src/industrial_core/industrial_robot_client/config/robot_state_visualize.vcg deleted file mode 100644 index abf87f64..00000000 --- a/training/supplements/src/industrial_core/industrial_robot_client/config/robot_state_visualize.vcg +++ /dev/null @@ -1,93 +0,0 @@ -Background\ ColorB=0.466667 -Background\ ColorG=0 -Background\ ColorR=0.0941176 -Camera\ Config=0.459797 4.57899 4.58262 0 0 0 -Camera\ Type=rviz::OrbitViewController -Fixed\ Frame=/base_link -Grid.Alpha=0.5 -Grid.Cell\ Size=0.5 -Grid.ColorB=0.898039 -Grid.ColorG=0.898039 -Grid.ColorR=0.898039 -Grid.Enabled=1 -Grid.Line\ Style=0 -Grid.Line\ Width=0.03 -Grid.Normal\ Cell\ Count=0 -Grid.OffsetX=0 -Grid.OffsetY=0 -Grid.OffsetZ=0 -Grid.Plane=0 -Grid.Plane\ Cell\ Count=10 -Grid.Reference\ Frame= -Property\ Grid\ Splitter=743,78 -Property\ Grid\ State=expanded=.Global Options,Grid.Enabled,Robot Model.Enabled,TF.Enabled;splitterratio=0.5 -QMainWindow=000000ff00000000fd00000003000000000000011d00000387fc0200000001fb000000100044006900730070006c006100790073010000001d00000387000000ee00ffffff000000010000013000000195fc0200000003fb0000001e0054006f006f006c002000500072006f0070006500720074006900650073000000001d000000670000006700fffffffb0000000a00560069006500770073000000001d00000101000000bb00fffffffb0000001200530065006c0065006300740069006f006e000000001d000001950000006700ffffff00000003000005000000003efc0100000001fb0000000800540069006d0065010000000000000500000002bf00ffffff000003dd0000038700000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 -Robot\ Model.Alpha=1 -Robot\ Model.Collision\ Enabled=0 -Robot\ Model.Enabled=1 -Robot\ Model.Robot\ Description=robot_description -Robot\ Model.TF\ Prefix= -Robot\ Model.Update\ Interval=0 -Robot\ Model.Visual\ Enabled=1 -Robot\:\ Robot\ Model\ Link\ base_linkAlpha=1 -Robot\:\ Robot\ Model\ Link\ base_linkEnabled=1 -Robot\:\ Robot\ Model\ Link\ base_linkShow\ Axes=0 -Robot\:\ Robot\ Model\ Link\ base_linkShow\ Trail=0 -Robot\:\ Robot\ Model\ Link\ link_1Alpha=1 -Robot\:\ Robot\ Model\ Link\ link_1Enabled=1 -Robot\:\ Robot\ Model\ Link\ link_1Show\ Axes=0 -Robot\:\ Robot\ Model\ Link\ link_1Show\ Trail=0 -Robot\:\ Robot\ Model\ Link\ link_2Alpha=1 -Robot\:\ Robot\ Model\ Link\ link_2Enabled=1 -Robot\:\ Robot\ Model\ Link\ link_2Show\ Axes=0 -Robot\:\ Robot\ Model\ Link\ link_2Show\ Trail=0 -Robot\:\ Robot\ Model\ Link\ link_3Alpha=1 -Robot\:\ Robot\ Model\ Link\ link_3Enabled=1 -Robot\:\ Robot\ Model\ Link\ link_3Show\ Axes=0 -Robot\:\ Robot\ Model\ Link\ link_3Show\ Trail=0 -Robot\:\ Robot\ Model\ Link\ link_4Alpha=1 -Robot\:\ Robot\ Model\ Link\ link_4Enabled=1 -Robot\:\ Robot\ Model\ Link\ link_4Show\ Axes=0 -Robot\:\ Robot\ Model\ Link\ link_4Show\ Trail=0 -Robot\:\ Robot\ Model\ Link\ link_5Alpha=1 -Robot\:\ Robot\ Model\ Link\ link_5Enabled=1 -Robot\:\ Robot\ Model\ Link\ link_5Show\ Axes=0 -Robot\:\ Robot\ Model\ Link\ link_5Show\ Trail=0 -Robot\:\ Robot\ Model\ Link\ link_6Alpha=1 -Robot\:\ Robot\ Model\ Link\ link_6Enabled=1 -Robot\:\ Robot\ Model\ Link\ link_6Show\ Axes=0 -Robot\:\ Robot\ Model\ Link\ link_6Show\ Trail=0 -TF.All\ Enabled=1 -TF.Enabled=1 -TF.Frame\ Timeout=15 -TF.Marker\ Scale=1 -TF.Show\ Arrows=1 -TF.Show\ Axes=1 -TF.Show\ Names=1 -TF.Update\ Interval=0 -Target\ Frame= -Tool\ 2D\ Nav\ GoalTopic=goal -Tool\ 2D\ Pose\ EstimateTopic=initialpose -[Display0] -ClassName=rviz::GridDisplay -Name=Grid -[Display1] -ClassName=rviz::RobotModelDisplay -Name=Robot Model -[Display2] -ClassName=rviz::TFDisplay -Name=TF -[TF.Frames.] -base_link.Enabled=1 -link_1.Enabled=1 -link_2.Enabled=1 -link_3.Enabled=1 -link_4.Enabled=1 -link_5.Enabled=1 -link_6.Enabled=1 -tool0.Enabled=1 -[Window] -Height=1000 -Width=1280 -X=-1 -Y=-29 diff --git a/training/supplements/src/industrial_core/industrial_robot_client/include/industrial_robot_client/joint_relay_handler.h b/training/supplements/src/industrial_core/industrial_robot_client/include/industrial_robot_client/joint_relay_handler.h deleted file mode 100644 index f15616b0..00000000 --- a/training/supplements/src/industrial_core/industrial_robot_client/include/industrial_robot_client/joint_relay_handler.h +++ /dev/null @@ -1,159 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2011, Southwest Research Institute - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in the - * documentation and/or other materials provided with the distribution. - * * Neither the name of the Southwest Research Institute, nor the names - * of its contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - - -#ifndef JOINT_HANDLER_H -#define JOINT_HANDLER_H - -#include -#include - -#include "ros/ros.h" -#include "control_msgs/FollowJointTrajectoryFeedback.h" -#include "sensor_msgs/JointState.h" -#include "simple_message/message_handler.h" -#include "simple_message/messages/joint_message.h" - - -namespace industrial_robot_client -{ -namespace joint_relay_handler -{ - -using industrial::joint_message::JointMessage; -using industrial::simple_message::SimpleMessage; - -/** - * \brief Message handler that relays joint positions (converts simple message - * types to ROS message types and publishes them) - * - * THIS CLASS IS NOT THREAD-SAFE - * - */ -class JointRelayHandler : public industrial::message_handler::MessageHandler -{ - // since this class defines a different init(), this helps find the base-class init() - using industrial::message_handler::MessageHandler::init; - -public: - - /** -* \brief Constructor -*/ - JointRelayHandler() {}; - - - /** - * \brief Class initializer - * - * \param connection simple message connection that will be used to send replies. - * \param joint_names list of joint-names for msg-publishing. - * - Count and order should match data from robot connection. - * - Use blank-name to exclude a joint from publishing. - * - * \return true on success, false otherwise (an invalid message type) - */ - bool init(industrial::smpl_msg_connection::SmplMsgConnection* connection, std::vector &joint_names); - -protected: - - std::vector all_joint_names_; - - ros::Publisher pub_joint_control_state_; - ros::Publisher pub_joint_sensor_state_; - ros::NodeHandle node_; - - /** - * \brief Convert joint message into publish message-types - * - * \param[in] msg_in Joint message from robot connection - * \param[out] control_state FollowJointTrajectoryFeedback message for ROS publishing - * \param[out] sensor_state JointState message for ROS publishing - * - * \return true on success, false otherwise - */ - virtual bool create_messages(JointMessage& msg_in, - control_msgs::FollowJointTrajectoryFeedback* control_state, - sensor_msgs::JointState* sensor_state); - - /** - * \brief Transform joint positions before publishing. - * Can be overridden to implement, e.g. robot-specific joint coupling. - * - * \param[in] pos_in joint positions, exactly as passed from robot connection. - * \param[out] pos_out transformed joint positions (in same order/count as input positions) - * - * \return true on success, false otherwise - */ - virtual bool transform(const std::vector& pos_in, std::vector* pos_out) - { - *pos_out = pos_in; // by default, no transform is applied - return true; - } - - /** - * \brief Select specific joints for publishing - * - * \param[in] all_joint_pos joint positions, in count/order matching robot connection - * \param[in] all_joint_names joint names, matching all_joint_pos - * \param[out] pub_joint_pos joint positions selected for publishing - * \param[out] pub_joint_names joint names selected for publishing - * - * \return true on success, false otherwise - */ - virtual bool select(const std::vector& all_joint_pos, const std::vector& all_joint_names, - std::vector* pub_joint_pos, std::vector* pub_joint_names); - - /** - * \brief Callback executed upon receiving a joint message - * - * \param in incoming message - * - * \return true on success, false otherwise - */ - bool internalCB(JointMessage& in); - -private: - /** - * \brief Callback executed upon receiving a message - * - * \param in incoming message - * - * \return true on success, false otherwise - */ - bool internalCB(SimpleMessage& in); -};//class JointRelayHandler - -}//joint_relay_handler -}//industrial_robot_cliet - - -#endif /* JOINT_HANDLER_H */ diff --git a/training/supplements/src/industrial_core/industrial_robot_client/include/industrial_robot_client/joint_trajectory_action.h b/training/supplements/src/industrial_core/industrial_robot_client/include/industrial_robot_client/joint_trajectory_action.h deleted file mode 100644 index c1718698..00000000 --- a/training/supplements/src/industrial_core/industrial_robot_client/include/industrial_robot_client/joint_trajectory_action.h +++ /dev/null @@ -1,230 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2012, Southwest Research Institute - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in the - * documentation and/or other materials provided with the distribution. - * * Neither the name of the Southwest Research Institute, nor the names - * of its contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef JOINT_TRAJTORY_ACTION_H -#define JOINT_TRAJTORY_ACTION_H - -#include -#include - -#include -#include -#include -#include - -namespace industrial_robot_client -{ -namespace joint_trajectory_action -{ - -class JointTrajectoryAction -{ - -public: - /** - * \brief Constructor - * - */ - JointTrajectoryAction(); - - /** - * \brief Destructor - * - */ - ~JointTrajectoryAction(); - - /** - * \brief Begin processing messages and publishing topics. - */ - void run() { ros::spin(); } - -private: - - typedef actionlib::ActionServer JointTractoryActionServer; - - /** - * \brief Internal ROS node handle - */ - ros::NodeHandle node_; - - /** - * \brief Internal action server - */ - JointTractoryActionServer action_server_; - - /** - * \brief Publishes desired trajectory (typically to the robot driver) - */ - ros::Publisher pub_trajectory_command_; - - /** - * \brief Subscribes to trajectory feedback (typically published by the - * robot driver). - */ - ros::Subscriber sub_trajectory_state_; - - /** - * \brief Subscribes to the robot status (typically published by the - * robot driver). - */ - ros::Subscriber sub_robot_status_; - - /** - * \brief Watchdog time used to fail the action request if the robot - * driver is not responding. - */ - ros::Timer watchdog_timer_; - - /** - * \brief Indicates action has an active goal - */ - bool has_active_goal_; - - /** - * \brief Cache of the current active goal - */ - JointTractoryActionServer::GoalHandle active_goal_; - /** - * \brief Cache of the current active trajectory - */ - trajectory_msgs::JointTrajectory current_traj_; - - /** - * \brief The default goal joint threshold see(goal_threshold). Unit - * are joint specific (i.e. radians or meters). - */ - static const double DEFAULT_GOAL_THRESHOLD_;// = 0.01; - - /** - * \brief The goal joint threshold used for determining if a robot - * is near it final destination. A single value is used for all joints - * - * NOTE: This value is used in conjunction with the robot inMotion - * status (see industrial_msgs::RobotStatus) if it exists. - */ - double goal_threshold_; - - /** - * \brief The joint names associated with the robot the action is - * interfacing with. The joint names must be the same as expected - * by the robot driver. - */ - std::vector joint_names_; - - /** - * \brief Cache of the last subscribed feedback message - */ - control_msgs::FollowJointTrajectoryFeedbackConstPtr last_trajectory_state_; - - /** - * \brief Indicates trajectory state has been received. Used by - * watchdog to determine if the robot driver is responding. - */ - bool trajectory_state_recvd_; - - /** - * \brief Cache of the last subscribed status message - */ - industrial_msgs::RobotStatusConstPtr last_robot_status_; - - /** - * \brief The watchdog period (seconds) - */ - static const double WATCHD0G_PERIOD_;// = 1.0; - - /** - * \brief Watch dog callback, used to detect robot driver failures - * - * \param e time event information - * - */ - void watchdog(const ros::TimerEvent &e); - - /** - * \brief Action server goal callback method - * - * \param gh goal handle - * - */ - void goalCB(JointTractoryActionServer::GoalHandle & gh); - - /** - * \brief Action server cancel callback method - * - * \param gh goal handle - * - */ - - void cancelCB(JointTractoryActionServer::GoalHandle & gh); - /** - * \brief Controller state callback (executed when feedback message - * received) - * - * \param msg joint trajectory feedback message - * - */ - void controllerStateCB(const control_msgs::FollowJointTrajectoryFeedbackConstPtr &msg); - - /** - * \brief Controller status callback (executed when robot status - * message received) - * - * \param msg robot status message - * - */ - void robotStatusCB(const industrial_msgs::RobotStatusConstPtr &msg); - - /** - * \brief Aborts the current action goal and sends a stop command - * (empty message) to the robot driver. - * - * - */ - void abortGoal(); - - /** - * \brief Controller status callback (executed when robot status - * message received) - * - * \param msg trajectory feedback message - * \param traj trajectory to test against feedback - * - * \return true if all joints are within goal contraints - * - */ - bool withinGoalConstraints(const control_msgs::FollowJointTrajectoryFeedbackConstPtr &msg, - const trajectory_msgs::JointTrajectory & traj); -}; - -} //joint_trajectory_action -} //industrial_robot_client - -#endif /* JOINT_TRAJTORY_ACTION_H */ diff --git a/training/supplements/src/industrial_core/industrial_robot_client/include/industrial_robot_client/joint_trajectory_downloader.h b/training/supplements/src/industrial_core/industrial_robot_client/include/industrial_robot_client/joint_trajectory_downloader.h deleted file mode 100644 index 0c8af507..00000000 --- a/training/supplements/src/industrial_core/industrial_robot_client/include/industrial_robot_client/joint_trajectory_downloader.h +++ /dev/null @@ -1,61 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2011, Southwest Research Institute - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in the - * documentation and/or other materials provided with the distribution. - * * Neither the name of the Southwest Research Institute, nor the names - * of its contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef JOINT_TRAJECTORY_DOWNLOADER_H -#define JOINT_TRAJECTORY_DOWNLOADER_H - -#include "industrial_robot_client/joint_trajectory_interface.h" - -namespace industrial_robot_client -{ -namespace joint_trajectory_downloader -{ - -using industrial_robot_client::joint_trajectory_interface::JointTrajectoryInterface; -using industrial::joint_traj_pt_message::JointTrajPtMessage; - -/** - * \brief Message handler that downloads joint trajectories to - * a robot controller that supports the trajectory downloading interface - */ -class JointTrajectoryDownloader : public JointTrajectoryInterface -{ - -public: - - bool send_to_robot(const std::vector& messages); - -}; - -} //joint_trajectory_downloader -} //industrial_robot_client - -#endif /* JOINT_TRAJECTORY_DOWNLOADER_H */ diff --git a/training/supplements/src/industrial_core/industrial_robot_client/include/industrial_robot_client/joint_trajectory_interface.h b/training/supplements/src/industrial_core/industrial_robot_client/include/industrial_robot_client/joint_trajectory_interface.h deleted file mode 100644 index f135cbda..00000000 --- a/training/supplements/src/industrial_core/industrial_robot_client/include/industrial_robot_client/joint_trajectory_interface.h +++ /dev/null @@ -1,277 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2011, Southwest Research Institute - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in the - * documentation and/or other materials provided with the distribution. - * * Neither the name of the Southwest Research Institute, nor the names - * of its contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef JOINT_TRAJECTORY_INTERFACE_H -#define JOINT_TRAJECTORY_INTERFACE_H - -#include -#include -#include - -#include "ros/ros.h" -#include "industrial_msgs/CmdJointTrajectory.h" -#include "industrial_msgs/StopMotion.h" -#include "sensor_msgs/JointState.h" -#include "simple_message/smpl_msg_connection.h" -#include "simple_message/socket/tcp_client.h" -#include "simple_message/messages/joint_traj_pt_message.h" -#include "trajectory_msgs/JointTrajectory.h" - -namespace industrial_robot_client -{ -namespace joint_trajectory_interface -{ - - using industrial::smpl_msg_connection::SmplMsgConnection; - using industrial::tcp_client::TcpClient; - using industrial::joint_traj_pt_message::JointTrajPtMessage; - namespace StandardSocketPorts = industrial::simple_socket::StandardSocketPorts; - -/** - * \brief Message handler that relays joint trajectories to the robot controller - * - * THIS CLASS IS NOT THREAD-SAFE - * - */ -class JointTrajectoryInterface -{ - -public: - - /** - * \brief Default constructor. - */ - JointTrajectoryInterface() : default_joint_pos_(0.0), default_vel_ratio_(0.1), default_duration_(10.0) {}; - - /** - * \brief Initialize robot connection using default method. - * - * \param default_ip default IP address to use for robot connection [OPTIONAL] - * - this value will be used if ROS param "robot_ip_address" cannot be read - * \param default_port default port to use for robot connection [OPTIONAL] - * - this value will be used if ROS param "~port" cannot be read - * - * \return true on success, false otherwise - */ - virtual bool init(std::string default_ip = "", int default_port = StandardSocketPorts::MOTION); - - - /** - * \brief Initialize robot connection using specified method. - * - * \param connection new robot-connection instance (ALREADY INITIALIZED). - * - * \return true on success, false otherwise - */ - virtual bool init(SmplMsgConnection* connection); - - /** - * \brief Class initializer - * - * \param connection simple message connection that will be used to send commands to robot (ALREADY INITIALIZED) - * \param joint_names list of expected joint-names. - * - Count and order should match data sent to robot connection. - * - Use blank-name to insert a placeholder joint position (typ. 0.0). - * - Joints in the incoming JointTrajectory stream that are NOT listed here will be ignored. - * \param velocity_limits map of maximum velocities for each joint - * - leave empty to lookup from URDF - * \return true on success, false otherwise (an invalid message type) - */ - virtual bool init(SmplMsgConnection* connection, const std::vector &joint_names, - const std::map &velocity_limits = std::map()); - - virtual ~JointTrajectoryInterface(); - - /** - * \brief Begin processing messages and publishing topics. - */ - virtual void run() { ros::spin(); } - -protected: - - /** - * \brief Send a stop command to the robot - */ - virtual void trajectoryStop(); - - /** - * \brief Convert ROS trajectory message into stream of JointTrajPtMessages for sending to robot. - * Also includes various joint transforms that can be overridden for robot-specific behavior. - * - * \param[in] traj ROS JointTrajectory message - * \param[out] msgs list of JointTrajPtMessages for sending to robot - * - * \return true on success, false otherwise - */ - virtual bool trajectory_to_msgs(const trajectory_msgs::JointTrajectoryConstPtr &traj, std::vector* msgs); - - /** - * \brief Transform joint positions before publishing. - * Can be overridden to implement, e.g. robot-specific joint coupling. - * - * \param[in] pt_in trajectory-point, in same order as expected for robot-connection. - * \param[out] pt_out transformed trajectory-point (in same order/count as input positions) - * - * \return true on success, false otherwise - */ - virtual bool transform(const trajectory_msgs::JointTrajectoryPoint& pt_in, trajectory_msgs::JointTrajectoryPoint* pt_out) - { - *pt_out = pt_in; // by default, no transform is applied - return true; - } - - /** - * \brief Select specific joints for sending to the robot - * - * \param[in] ros_joint_names joint names from ROS command - * \param[in] ros_pt target pos/vel from ROS command - * \param[in] rbt_joint_names joint names, in order/count expected by robot connection - * \param[out] rbt_pt target pos/vel, matching rbt_joint_names - * - * \return true on success, false otherwise - */ - virtual bool select(const std::vector& ros_joint_names, const trajectory_msgs::JointTrajectoryPoint& ros_pt, - const std::vector& rbt_joint_names, trajectory_msgs::JointTrajectoryPoint* rbt_pt); - - /** - * \brief Reduce the ROS velocity commands (per-joint velocities) to a single scalar for communication to the robot. - * For flexibility, the robot command message contains both "velocity" and "duration" fields. The specific robot - * implementation can utilize either or both of these fields, as appropriate. - * - * \param[in] pt trajectory point data, in order/count expected by robot connection - * \param[out] rbt_velocity computed velocity scalar for robot message (if needed by robot) - * \param[out] rbt_duration computed move duration for robot message (if needed by robot) - * - * \return true on success, false otherwise - */ - virtual bool calc_speed(const trajectory_msgs::JointTrajectoryPoint& pt, double* rbt_velocity, double* rbt_duration); - - /** - * \brief Reduce the ROS velocity commands (per-joint velocities) to a single scalar for communication to the robot. - * If unneeded by the robot server, set to 0 (or any value). - * - * \param[in] pt trajectory point data, in order/count expected by robot connection - * \param[out] rbt_velocity computed velocity scalar for robot message (if needed by robot) - * - * \return true on success, false otherwise - */ - virtual bool calc_velocity(const trajectory_msgs::JointTrajectoryPoint& pt, double* rbt_velocity); - - /** - * \brief Compute the expected move duration for communication to the robot. - * If unneeded by the robot server, set to 0 (or any value). - * - * \param[in] pt trajectory point data, in order/count expected by robot connection - * \param[out] rbt_duration computed move duration for robot message (if needed by robot) - * - * \return true on success, false otherwise - */ - virtual bool calc_duration(const trajectory_msgs::JointTrajectoryPoint& pt, double* rbt_duration); - - /** - * \brief Send trajectory to robot, using this node's robot-connection. - * Specific method must be implemented in a derived class (e.g. streaming, download, etc.) - * - * \param messages List of SimpleMessage JointTrajPtMessages to send to robot. - * - * \return true on success, false otherwise - */ - virtual bool send_to_robot(const std::vector& messages)=0; - - /** - * \brief Callback function registered to ROS topic-subscribe. - * Transform message into SimpleMessage objects and send commands to robot. - * - * \param msg JointTrajectory message from ROS trajectory-planner - */ - virtual void jointTrajectoryCB(const trajectory_msgs::JointTrajectoryConstPtr &msg); - - /** - * \brief Callback function registered to ROS stopMotion service - * Sends stop-motion command to robot. - * - * \param req StopMotion request from service call - * \param res StopMotion response to service call - * \return true always. Look at res.code.val to see if call actually succeeded. - */ - virtual bool stopMotionCB(industrial_msgs::StopMotion::Request &req, - industrial_msgs::StopMotion::Response &res); - - /** - * \brief Validate that trajectory command meets minimum requirements - * - * \param traj incoming trajectory - * \return true if trajectory is valid, false otherwise - */ - virtual bool is_valid(const trajectory_msgs::JointTrajectory &traj); - - /* - * \brief Callback for JointState topic - * - * \param msg JointState message - */ - virtual void jointStateCB(const sensor_msgs::JointStateConstPtr &msg); - - TcpClient default_tcp_connection_; - - ros::NodeHandle node_; - SmplMsgConnection* connection_; - ros::Subscriber sub_cur_pos_; // handle for joint-state topic subscription - ros::Subscriber sub_joint_trajectory_; // handle for joint-trajectory topic subscription - ros::ServiceServer srv_joint_trajectory_; // handle for joint-trajectory service - ros::ServiceServer srv_stop_motion_; // handle for stop_motion service - std::vector all_joint_names_; - double default_joint_pos_; // default position to use for "dummy joints", if none specified - double default_vel_ratio_; // default velocity ratio to use for joint commands, if no velocity or max_vel specified - double default_duration_; // default duration to use for joint commands, if no - std::map joint_vel_limits_; // cache of max joint velocities from URDF - sensor_msgs::JointState cur_joint_pos_; // cache of last received joint state - - -private: - static JointTrajPtMessage create_message(int seq, std::vector joint_pos, double velocity, double duration); - - /** - * \brief Callback function registered to ROS CmdJointTrajectory service - * Duplicates message-topic functionality, but in service form. - * - * \param req CmdJointTrajectory request from service call - * \param res CmdJointTrajectory response to service call - * \return true always. Look at res.code.val to see if call actually succeeded - */ - bool jointTrajectoryCB(industrial_msgs::CmdJointTrajectory::Request &req, - industrial_msgs::CmdJointTrajectory::Response &res); -}; - -} //joint_trajectory_interface -} //industrial_robot_client - -#endif /* JOINT_TRAJECTORY_INTERFACE_H */ diff --git a/training/supplements/src/industrial_core/industrial_robot_client/include/industrial_robot_client/joint_trajectory_streamer.h b/training/supplements/src/industrial_core/industrial_robot_client/include/industrial_robot_client/joint_trajectory_streamer.h deleted file mode 100644 index 87b5fcda..00000000 --- a/training/supplements/src/industrial_core/industrial_robot_client/include/industrial_robot_client/joint_trajectory_streamer.h +++ /dev/null @@ -1,122 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2011, Southwest Research Institute - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in the - * documentation and/or other materials provided with the distribution. - * * Neither the name of the Southwest Research Institute, nor the names - * of its contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef JOINT_TRAJECTORY_STREAMER_H -#define JOINT_TRAJECTORY_STREAMER_H - -#include -#include "industrial_robot_client/joint_trajectory_interface.h" - -namespace industrial_robot_client -{ -namespace joint_trajectory_streamer -{ - -using industrial_robot_client::joint_trajectory_interface::JointTrajectoryInterface; -using industrial::joint_traj_pt_message::JointTrajPtMessage; -using industrial::smpl_msg_connection::SmplMsgConnection; - -namespace TransferStates -{ -enum TransferState -{ - IDLE = 0, STREAMING =1 //,STARTING, //, STOPPING -}; -} -typedef TransferStates::TransferState TransferState; - -/** - * \brief Message handler that streams joint trajectories to the robot controller - */ - -//* JointTrajectoryStreamer -/** - * - * THIS CLASS IS NOT THREAD-SAFE - * - */ -class JointTrajectoryStreamer : public JointTrajectoryInterface -{ - -public: - - // since this class defines a different init(), this helps find the base-class init() - using JointTrajectoryInterface::init; - - /** - * \brief Default constructor - * - * \param min_buffer_size minimum number of points as required by robot implementation - */ - JointTrajectoryStreamer(int min_buffer_size = 1) : min_buffer_size_(min_buffer_size) {}; - - /** - * \brief Class initializer - * - * \param connection simple message connection that will be used to send commands to robot (ALREADY INITIALIZED) - * \param joint_names list of expected joint-names. - * - Count and order should match data sent to robot connection. - * - Use blank-name to insert a placeholder joint position (typ. 0.0). - * - Joints in the incoming JointTrajectory stream that are NOT listed here will be ignored. - * \param velocity_limits map of maximum velocities for each joint - * - leave empty to lookup from URDF - * \return true on success, false otherwise (an invalid message type) - */ - virtual bool init(SmplMsgConnection* connection, const std::vector &joint_names, - const std::map &velocity_limits = std::map()); - - ~JointTrajectoryStreamer(); - - virtual void jointTrajectoryCB(const trajectory_msgs::JointTrajectoryConstPtr &msg); - - virtual bool trajectory_to_msgs(const trajectory_msgs::JointTrajectoryConstPtr &traj, std::vector* msgs); - - void streamingThread(); - - bool send_to_robot(const std::vector& messages); - -protected: - - void trajectoryStop(); - - boost::thread* streaming_thread_; - boost::mutex mutex_; - int current_point_; - std::vector current_traj_; - TransferState state_; - ros::Time streaming_start_; - int min_buffer_size_; -}; - -} //joint_trajectory_streamer -} //industrial_robot_client - -#endif /* JOINT_TRAJECTORY_STREAMER_H */ diff --git a/training/supplements/src/industrial_core/industrial_robot_client/include/industrial_robot_client/robot_state_interface.h b/training/supplements/src/industrial_core/industrial_robot_client/include/industrial_robot_client/robot_state_interface.h deleted file mode 100644 index f052becf..00000000 --- a/training/supplements/src/industrial_core/industrial_robot_client/include/industrial_robot_client/robot_state_interface.h +++ /dev/null @@ -1,167 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2012, Southwest Research Institute - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in the - * documentation and/or other materials provided with the distribution. - * * Neither the name of the Southwest Research Institute, nor the names - * of its contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - - -#ifndef ROBOT_STATE_INTERFACE_H -#define ROBOT_STATE_INTERFACE_H - -#include -#include -#include "simple_message/smpl_msg_connection.h" -#include "simple_message/message_manager.h" -#include "simple_message/message_handler.h" -#include "simple_message/socket/tcp_client.h" -#include "industrial_robot_client/joint_relay_handler.h" -#include "industrial_robot_client/robot_status_relay_handler.h" - -namespace industrial_robot_client -{ -namespace robot_state_interface -{ - -using industrial::smpl_msg_connection::SmplMsgConnection; -using industrial::message_manager::MessageManager; -using industrial::message_handler::MessageHandler; -using industrial::tcp_client::TcpClient; -using industrial_robot_client::joint_relay_handler::JointRelayHandler; -using industrial_robot_client::robot_status_relay_handler::RobotStatusRelayHandler; -namespace StandardSocketPorts = industrial::simple_socket::StandardSocketPorts; - -/** - * \brief Generic template that reads state-data from a robot controller - * and publishes matching messages to various ROS topics. - * - * Users should replace the default class members - * to implement robot-specific behavior. - */ -//* RobotStateInterface -class RobotStateInterface -{ - -public: - - /** - * \brief Default constructor. - */ - RobotStateInterface(); - - /** - * \brief Initialize robot connection using default method. - * - * \param default_ip default IP address to use for robot connection [OPTIONAL] - * - this value will be used if ROS param "robot_ip_address" cannot be read - * \param default_port default port to use for robot connection [OPTIONAL] - * - this value will be used if ROS param "~port" cannot be read - * - * \return true on success, false otherwise - */ - bool init(std::string default_ip = "", int default_port = StandardSocketPorts::STATE); - - - /** - * \brief Initialize robot connection using specified method. - * - * \param connection new robot-connection instance (ALREADY INITIALIZED). - * - * \return true on success, false otherwise - */ - bool init(SmplMsgConnection* connection); - - /** - * \brief Initialize robot connection using specified method and joint-names. - * - * \param connection new robot-connection instance (ALREADY INITIALIZED). - * \param joint_names list of joint-names for ROS topic - * - Count and order should match data sent to robot connection. - * - Use blank-name to skip (not publish) a joint-position - * - * \return true on success, false otherwise - */ - bool init(SmplMsgConnection* connection, std::vector& joint_names); - - /** - * \brief Begin processing messages and publishing topics. - */ - void run(); - - /** - * \brief get current robot-connection instance. - * - * \return current robot connection object - */ - SmplMsgConnection* get_connection() - { - return this->connection_; - } - - /** - * \brief get active message-manager object - * - * \return current message-manager object - */ - MessageManager* get_manager() - { - return &this->manager_; - } - - std::vector get_joint_names() - { - return this->joint_names_; - } - - - /** - * \brief Add a new handler. - * - * \param new message-handler for a specific msg-type (ALREADY INITIALIZED). - * \param replace existing handler (of same msg-type), if exists - */ - void add_handler(MessageHandler* handler, bool allow_replace = true) - { - this->manager_.add(handler, allow_replace); - } - -protected: - TcpClient default_tcp_connection_; - JointRelayHandler default_joint_handler_; - RobotStatusRelayHandler default_robot_status_handler_; - - SmplMsgConnection* connection_; - MessageManager manager_; - std::vector joint_names_; - -};//class RobotStateInterface - -}//robot_state_interface -}//industrial_robot_cliet - - -#endif /* ROBOT_STATE_INTERFACE_H */ diff --git a/training/supplements/src/industrial_core/industrial_robot_client/include/industrial_robot_client/robot_status_relay_handler.h b/training/supplements/src/industrial_core/industrial_robot_client/include/industrial_robot_client/robot_status_relay_handler.h deleted file mode 100644 index 47338aae..00000000 --- a/training/supplements/src/industrial_core/industrial_robot_client/include/industrial_robot_client/robot_status_relay_handler.h +++ /dev/null @@ -1,104 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2012, Southwest Research Institute - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in the - * documentation and/or other materials provided with the distribution. - * * Neither the name of the Southwest Research Institute, nor the names - * of its contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - - -#ifndef ROBOT_STATUS_RELAY_HANDLER_H -#define ROBOT_STATUS_RELAY_HANDLER_H - -#include "ros/ros.h" -#include "simple_message/message_handler.h" -#include "simple_message/messages/robot_status_message.h" - - -namespace industrial_robot_client -{ -namespace robot_status_relay_handler -{ - -/** - * \brief Message handler that relays joint positions (converts simple message - * types to ROS message types and publishes them) - * - * THIS CLASS IS NOT THREAD-SAFE - * - */ -class RobotStatusRelayHandler : public industrial::message_handler::MessageHandler -{ - // since this class defines a different init(), this helps find the base-class init() - using industrial::message_handler::MessageHandler::init; - -public: - - /** -* \brief Constructor -*/ - RobotStatusRelayHandler() {}; - - - /** - * \brief Class initializer - * - * \param connection simple message connection that will be used to send replies. - * - * \return true on success, false otherwise (an invalid message type) - */ - bool init(industrial::smpl_msg_connection::SmplMsgConnection* connection); - -protected: - - ros::Publisher pub_robot_status_; - ros::NodeHandle node_; - - /** - * \brief Callback executed upon receiving a robot status message - * - * \param in incoming message - * - * \return true on success, false otherwise - */ - bool internalCB(industrial::robot_status_message::RobotStatusMessage & in); - -private: - /** - * \brief Callback executed upon receiving a message - * - * \param in incoming message - * - * \return true on success, false otherwise - */ - bool internalCB(industrial::simple_message::SimpleMessage& in); -}; - -} -} - - -#endif /* ROBOT_STATUS_RELAY_HANDLER_H */ diff --git a/training/supplements/src/industrial_core/industrial_robot_client/include/industrial_robot_client/utils.h b/training/supplements/src/industrial_core/industrial_robot_client/include/industrial_robot_client/utils.h deleted file mode 100644 index a0af2c6e..00000000 --- a/training/supplements/src/industrial_core/industrial_robot_client/include/industrial_robot_client/utils.h +++ /dev/null @@ -1,122 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2012, Southwest Research Institute - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in the - * documentation and/or other materials provided with the distribution. - * * Neither the name of the Southwest Research Institute, nor the names - * of its contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef INDUSTRIAL_ROBOT_CLIENT_UTILS_H_ -#define INDUSTRIAL_ROBOT_CLIENT_UTILS_H_ - -#include -#include -#include - -namespace industrial_robot_client -{ -namespace utils -{ - - -/** - * \brief Checks to see if members are within the same range. Specifically, - * Each item in abs(lhs[i] - rhs[i]) < abs(range/2.0). Empty vectors always - * return true. - * The behavior of this function around large values (near double limits) is - * uncertain. - * This function is not optimized, do not use for large vectors or loops. - * - * \param lhs first vector - * \param rhs second vector - * \param full_range - * - * \return true set are similar (same members, same order) - */ -bool isWithinRange(const std::vector & lhs, const std::vector & rhs, - double full_range); - -/** - * \brief Convenience function for inserting a value into an std::map, assuming - * a string key and a double value - * - * \param key - * \param value - * \param map - * - * \return true if insertion successful (or if key already exists) - */ -bool mapInsert(const std::string & key, double value, std::map & mappings); - - -/** - * \brief Convenience function for taking two separate key,value vectors and creating - * the more convenient map. This is helpful for some ROS message types where maps are - * defined as two separate vectors (ROS messages do not support maps). - * - * \param keys vector of key values - * \param values vector of values - * \param mappings of key values. - * - * \return true if conversion successful - */ -bool toMap(const std::vector & keys, const std::vector & values, - std::map & mappings); - - -/** - * \brief Map version of @see isWithinRange - * - * \param keys vector of key values - * \param lhs first map - * \param rhs second map - * \param full_range - * - * \return true if values are within range - */ -bool isWithinRange(const std::vector & keys, const std::map & lhs, - const std::map & rhs, double full_range); - - -/** - * \brief Key/Values vector version of @see isWithinRange - * - * \param lhs_keys vector of lhs keys - * \param lhs_values vector of lhs_values - * \param rhs_keys vector of rhs keys - * \param rhs_values vector of rhs_values - * \param full_range - * - * \return true values within range - */ -bool isWithinRange(const std::vector & lhs_keys, const std::vector & lhs_values, - const std::vector & rhs_keys, const std::vector & rhs_values, - double full_range); - -} //utils -} //industrial_robot_client - -#endif /* INDUSTRIAL_ROBOT_CLIENT_UTILS_H_ */ diff --git a/training/supplements/src/industrial_core/industrial_robot_client/launch/robot_interface_download.launch b/training/supplements/src/industrial_core/industrial_robot_client/launch/robot_interface_download.launch deleted file mode 100644 index ef433bab..00000000 --- a/training/supplements/src/industrial_core/industrial_robot_client/launch/robot_interface_download.launch +++ /dev/null @@ -1,35 +0,0 @@ - - - - - - - - - - - - - - - - - - - - diff --git a/training/supplements/src/industrial_core/industrial_robot_client/launch/robot_interface_streaming.launch b/training/supplements/src/industrial_core/industrial_robot_client/launch/robot_interface_streaming.launch deleted file mode 100644 index d72c5b17..00000000 --- a/training/supplements/src/industrial_core/industrial_robot_client/launch/robot_interface_streaming.launch +++ /dev/null @@ -1,36 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - diff --git a/training/supplements/src/industrial_core/industrial_robot_client/launch/robot_state_visualize.launch b/training/supplements/src/industrial_core/industrial_robot_client/launch/robot_state_visualize.launch deleted file mode 100644 index f23be749..00000000 --- a/training/supplements/src/industrial_core/industrial_robot_client/launch/robot_state_visualize.launch +++ /dev/null @@ -1,29 +0,0 @@ - - - - - - - - - - - - - - - - - diff --git a/training/supplements/src/industrial_core/industrial_robot_client/mainpage.dox b/training/supplements/src/industrial_core/industrial_robot_client/mainpage.dox deleted file mode 100644 index 81ac63f3..00000000 --- a/training/supplements/src/industrial_core/industrial_robot_client/mainpage.dox +++ /dev/null @@ -1,14 +0,0 @@ -/** -\mainpage -\htmlinclude manifest.html - -\b industrial_robot_client - - - ---> - - -*/ diff --git a/training/supplements/src/industrial_core/industrial_robot_client/package.xml b/training/supplements/src/industrial_core/industrial_robot_client/package.xml deleted file mode 100644 index 2d8bdd7b..00000000 --- a/training/supplements/src/industrial_core/industrial_robot_client/package.xml +++ /dev/null @@ -1,37 +0,0 @@ - - industrial_robot_client - 0.3.3 - industrial robot client contains generic clients for connecting - to industrial robot controllers with servers that adhere to the - simple message protocol. - - - Shaun Edwards - BSD - http://ros.org/wiki/industrial_robot_client - Jeremy Zoss - - catkin - roscpp - std_msgs - sensor_msgs - control_msgs - trajectory_msgs - simple_message - actionlib_msgs - actionlib - urdf - industrial_msgs - industrial_utils - roscpp - std_msgs - sensor_msgs - control_msgs - trajectory_msgs - simple_message - actionlib_msgs - actionlib - urdf - industrial_msgs - industrial_utils - diff --git a/training/supplements/src/industrial_core/industrial_robot_client/src/generic_joint_downloader_node.cpp b/training/supplements/src/industrial_core/industrial_robot_client/src/generic_joint_downloader_node.cpp deleted file mode 100644 index 7a447800..00000000 --- a/training/supplements/src/industrial_core/industrial_robot_client/src/generic_joint_downloader_node.cpp +++ /dev/null @@ -1,47 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2012, Southwest Research Institute - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in the - * documentation and/or other materials provided with the distribution. - * * Neither the name of the Southwest Research Institute, nor the names - * of its contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#include "industrial_robot_client/joint_trajectory_downloader.h" - -using industrial_robot_client::joint_trajectory_downloader::JointTrajectoryDownloader; - -int main(int argc, char** argv) -{ - // initialize node - ros::init(argc, argv, "motion_interface"); - - // launch the default JointTrajectoryDownloader connection/handlers - JointTrajectoryDownloader motionInterface; - motionInterface.init(); - motionInterface.run(); - - return 0; -} diff --git a/training/supplements/src/industrial_core/industrial_robot_client/src/generic_joint_streamer_node.cpp b/training/supplements/src/industrial_core/industrial_robot_client/src/generic_joint_streamer_node.cpp deleted file mode 100644 index a4e1451b..00000000 --- a/training/supplements/src/industrial_core/industrial_robot_client/src/generic_joint_streamer_node.cpp +++ /dev/null @@ -1,47 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2012, Southwest Research Institute - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in the - * documentation and/or other materials provided with the distribution. - * * Neither the name of the Southwest Research Institute, nor the names - * of its contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#include "industrial_robot_client/joint_trajectory_streamer.h" - -using industrial_robot_client::joint_trajectory_streamer::JointTrajectoryStreamer; - -int main(int argc, char** argv) -{ - // initialize node - ros::init(argc, argv, "motion_interface"); - - // launch the default JointTrajectoryStreamer connection/handlers - JointTrajectoryStreamer motionInterface; - motionInterface.init(); - motionInterface.run(); - - return 0; -} diff --git a/training/supplements/src/industrial_core/industrial_robot_client/src/generic_joint_trajectory_action_node.cpp b/training/supplements/src/industrial_core/industrial_robot_client/src/generic_joint_trajectory_action_node.cpp deleted file mode 100644 index 40d25559..00000000 --- a/training/supplements/src/industrial_core/industrial_robot_client/src/generic_joint_trajectory_action_node.cpp +++ /dev/null @@ -1,45 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2012, Southwest Research Institute - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in the - * documentation and/or other materials provided with the distribution. - * * Neither the name of the Southwest Research Institute, nor the names - * of its contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#include "industrial_robot_client/joint_trajectory_action.h" - -using industrial_robot_client::joint_trajectory_action::JointTrajectoryAction; - -int main(int argc, char** argv) -{ - // initialize node - ros::init(argc, argv, "joint_trajectory_action"); - - JointTrajectoryAction action; - action.run(); - - return 0; -} diff --git a/training/supplements/src/industrial_core/industrial_robot_client/src/generic_robot_state_node.cpp b/training/supplements/src/industrial_core/industrial_robot_client/src/generic_robot_state_node.cpp deleted file mode 100644 index e0dac616..00000000 --- a/training/supplements/src/industrial_core/industrial_robot_client/src/generic_robot_state_node.cpp +++ /dev/null @@ -1,48 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2012, Southwest Research Institute - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in the - * documentation and/or other materials provided with the distribution. - * * Neither the name of the Southwest Research Institute, nor the names - * of its contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#include "industrial_robot_client/robot_state_interface.h" - -using industrial_robot_client::robot_state_interface::RobotStateInterface; - -int main(int argc, char** argv) -{ - // initialize node - ros::init(argc, argv, "state_interface"); - - // launch the default Robot State Interface connection/handlers - RobotStateInterface rsi; - if (rsi.init()) - { - rsi.run(); - } - return 0; -} diff --git a/training/supplements/src/industrial_core/industrial_robot_client/src/joint_relay_handler.cpp b/training/supplements/src/industrial_core/industrial_robot_client/src/joint_relay_handler.cpp deleted file mode 100644 index 6d0741af..00000000 --- a/training/supplements/src/industrial_core/industrial_robot_client/src/joint_relay_handler.cpp +++ /dev/null @@ -1,172 +0,0 @@ -/* -* Software License Agreement (BSD License) -* -* Copyright (c) 2011, Southwest Research Institute -* All rights reserved. -* -* Redistribution and use in source and binary forms, with or without -* modification, are permitted provided that the following conditions are met: -* -* * Redistributions of source code must retain the above copyright -* notice, this list of conditions and the following disclaimer. -* * Redistributions in binary form must reproduce the above copyright -* notice, this list of conditions and the following disclaimer in the -* documentation and/or other materials provided with the distribution. -* * Neither the name of the Southwest Research Institute, nor the names -* of its contributors may be used to endorse or promote products derived -* from this software without specific prior written permission. -* -* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE -* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE -* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR -* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF -* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS -* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN -* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) -* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -* POSSIBILITY OF SUCH DAMAGE. -*/ - -#include - -#include "industrial_robot_client/joint_relay_handler.h" -#include "simple_message/log_wrapper.h" - -using industrial::shared_types::shared_real; -using industrial::smpl_msg_connection::SmplMsgConnection; -using namespace industrial::simple_message; - -namespace industrial_robot_client -{ -namespace joint_relay_handler -{ - -bool JointRelayHandler::init(SmplMsgConnection* connection, std::vector& joint_names) -{ - this->pub_joint_control_state_ = - this->node_.advertise("feedback_states", 1); - - this->pub_joint_sensor_state_ = this->node_.advertise("joint_states",1); - - // save "complete" joint-name list, preserving any blank entries for later use - this->all_joint_names_ = joint_names; - - return init((int)StandardMsgTypes::JOINT, connection); -} - -bool JointRelayHandler::internalCB(SimpleMessage& in) -{ - JointMessage joint_msg; - - if (!joint_msg.init(in)) - { - LOG_ERROR("Failed to initialize joint message"); - return false; - } - - return internalCB(joint_msg); -} - -bool JointRelayHandler::internalCB(JointMessage& in) -{ - control_msgs::FollowJointTrajectoryFeedback control_state; - sensor_msgs::JointState sensor_state; - bool rtn = true; - - if (create_messages(in, &control_state, &sensor_state)) - { - this->pub_joint_control_state_.publish(control_state); - this->pub_joint_sensor_state_.publish(sensor_state); - } - else - rtn = false; - - // Reply back to the controller if the sender requested it. - if (CommTypes::SERVICE_REQUEST == in.getMessageType()) - { - SimpleMessage reply; - in.toReply(reply, rtn ? ReplyTypes::SUCCESS : ReplyTypes::FAILURE); - this->getConnection()->sendMsg(reply); - } - - return rtn; -} - -// TODO: Add support for other message fields (velocity, effort, desired pos) -bool JointRelayHandler::create_messages(JointMessage& msg_in, - control_msgs::FollowJointTrajectoryFeedback* control_state, - sensor_msgs::JointState* sensor_state) -{ - // read joint positions from JointMessage - std::vector all_joint_pos(all_joint_names_.size()); - for (int i=0; i xform_joint_pos; - if (!transform(all_joint_pos, &xform_joint_pos)) - { - LOG_ERROR("Failed to transform joint positions"); - return false; - } - - // select specific joints for publishing - std::vector pub_joint_pos; - std::vector pub_joint_names; - if (!select(xform_joint_pos, all_joint_names_, &pub_joint_pos, &pub_joint_names)) - { - LOG_ERROR("Failed to select joints for publishing"); - return false; - } - - // assign values to messages - control_msgs::FollowJointTrajectoryFeedback tmp_control_state; // always start with a "clean" message - tmp_control_state.header.stamp = ros::Time::now(); - tmp_control_state.joint_names = pub_joint_names; - tmp_control_state.actual.positions = pub_joint_pos; - *control_state = tmp_control_state; - - sensor_msgs::JointState tmp_sensor_state; - tmp_sensor_state.header.stamp = ros::Time::now(); - tmp_sensor_state.name = pub_joint_names; - tmp_sensor_state.position = pub_joint_pos; - *sensor_state = tmp_sensor_state; - - return true; -} - -bool JointRelayHandler::select(const std::vector& all_joint_pos, const std::vector& all_joint_names, - std::vector* pub_joint_pos, std::vector* pub_joint_names) -{ - ROS_ASSERT(all_joint_pos.size() == all_joint_names.size()); - - pub_joint_pos->clear(); - pub_joint_names->clear(); - - // skip over "blank" joint names - for (int i=0; ipush_back(all_joint_pos[i]); - pub_joint_names->push_back(all_joint_names[i]); - } - - return true; -} - -}//namespace joint_relay_handler -}//namespace industrial_robot_client - - - - diff --git a/training/supplements/src/industrial_core/industrial_robot_client/src/joint_trajectory_action.cpp b/training/supplements/src/industrial_core/industrial_robot_client/src/joint_trajectory_action.cpp deleted file mode 100644 index b0c8031b..00000000 --- a/training/supplements/src/industrial_core/industrial_robot_client/src/joint_trajectory_action.cpp +++ /dev/null @@ -1,308 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2011, Southwest Research Institute - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in the - * documentation and/or other materials provided with the distribution. - * * Neither the name of the Southwest Research Institute, nor the names - * of its contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#include -#include -#include -#include - -namespace industrial_robot_client -{ -namespace joint_trajectory_action -{ - -const double JointTrajectoryAction::WATCHD0G_PERIOD_ = 1.0; -const double JointTrajectoryAction::DEFAULT_GOAL_THRESHOLD_ = 0.01; - -JointTrajectoryAction::JointTrajectoryAction() : - action_server_(node_, "joint_trajectory_action", boost::bind(&JointTrajectoryAction::goalCB, this, _1), - boost::bind(&JointTrajectoryAction::cancelCB, this, _1), false), has_active_goal_(false) -{ - ros::NodeHandle pn("~"); - - pn.param("constraints/goal_threshold", goal_threshold_, DEFAULT_GOAL_THRESHOLD_); - - if (!industrial_utils::param::getJointNames("controller_joint_names", "robot_description", joint_names_)) - ROS_ERROR("Failed to initialize joint_names."); - - // The controller joint names parameter includes empty joint names for those joints not supported - // by the controller. These are removed since the trajectory action should ignore these. - std::remove(joint_names_.begin(), joint_names_.end(), std::string()); - ROS_INFO_STREAM("Filtered joint names to " << joint_names_.size() << " joints"); - - pub_trajectory_command_ = node_.advertise("joint_path_command", 1); - sub_trajectory_state_ = node_.subscribe("feedback_states", 1, &JointTrajectoryAction::controllerStateCB, this); - sub_robot_status_ = node_.subscribe("robot_status", 1, &JointTrajectoryAction::robotStatusCB, this); - - watchdog_timer_ = node_.createTimer(ros::Duration(WATCHD0G_PERIOD_), &JointTrajectoryAction::watchdog, this); - action_server_.start(); -} - -JointTrajectoryAction::~JointTrajectoryAction() -{ -} - -void JointTrajectoryAction::robotStatusCB(const industrial_msgs::RobotStatusConstPtr &msg) -{ - last_robot_status_ = msg; //caching robot status for later use. -} - -void JointTrajectoryAction::watchdog(const ros::TimerEvent &e) -{ - // Some debug logging - if (!last_trajectory_state_) - { - ROS_DEBUG("Waiting for subscription to joint trajectory state"); - } - if (!trajectory_state_recvd_) - { - ROS_DEBUG("Trajectory state not received since last watchdog"); - } - - // Aborts the active goal if the controller does not appear to be active. - if (has_active_goal_) - { - if (!trajectory_state_recvd_) - { - // last_trajectory_state_ is null if the subscriber never makes a connection - if (!last_trajectory_state_) - { - ROS_WARN("Aborting goal because we have never heard a controller state message."); - } - else - { - ROS_WARN_STREAM( - "Aborting goal because we haven't heard from the controller in " << WATCHD0G_PERIOD_ << " seconds"); - } - - abortGoal(); - - } - } - - // Reset the trajectory state received flag - trajectory_state_recvd_ = false; -} - -void JointTrajectoryAction::goalCB(JointTractoryActionServer::GoalHandle & gh) -{ - ROS_INFO("Received new goal"); - - if (!gh.getGoal()->trajectory.points.empty()) - { - if (industrial_utils::isSimilar(joint_names_, gh.getGoal()->trajectory.joint_names)) - { - - // Cancels the currently active goal. - if (has_active_goal_) - { - ROS_WARN("Received new goal, canceling current goal"); - abortGoal(); - } - - // Sends the trajectory along to the controller - if (withinGoalConstraints(last_trajectory_state_, gh.getGoal()->trajectory)) - { - - ROS_INFO_STREAM("Already within goal constraints, setting goal succeeded"); - gh.setAccepted(); - gh.setSucceeded(); - has_active_goal_ = false; - - } - else - { - gh.setAccepted(); - active_goal_ = gh; - has_active_goal_ = true; - - ROS_INFO("Publishing trajectory"); - - current_traj_ = active_goal_.getGoal()->trajectory; - pub_trajectory_command_.publish(current_traj_); - } - } - else - { - ROS_ERROR("Joint trajectory action failing on invalid joints"); - control_msgs::FollowJointTrajectoryResult rslt; - rslt.error_code = control_msgs::FollowJointTrajectoryResult::INVALID_JOINTS; - gh.setRejected(rslt, "Joint names do not match"); - } - } - else - { - ROS_ERROR("Joint trajectory action failed on empty trajectory"); - control_msgs::FollowJointTrajectoryResult rslt; - rslt.error_code = control_msgs::FollowJointTrajectoryResult::INVALID_GOAL; - gh.setRejected(rslt, "Empty trajectory"); - } - - // Adding some informational log messages to indicate unsupported goal constraints - if (gh.getGoal()->goal_time_tolerance.toSec() > 0.0) - { - ROS_WARN_STREAM("Ignoring goal time tolerance in action goal, may be supported in the future"); - } - if (!gh.getGoal()->goal_tolerance.empty()) - { - ROS_WARN_STREAM( - "Ignoring goal tolerance in action, using paramater tolerance of " << goal_threshold_ << " instead"); - } - if (!gh.getGoal()->path_tolerance.empty()) - { - ROS_WARN_STREAM("Ignoring goal path tolerance, option not supported by ROS-Industrial drivers"); - } -} - -void JointTrajectoryAction::cancelCB(JointTractoryActionServer::GoalHandle & gh) -{ - ROS_DEBUG("Received action cancel request"); - if (active_goal_ == gh) - { - // Stops the controller. - trajectory_msgs::JointTrajectory empty; - empty.joint_names = joint_names_; - pub_trajectory_command_.publish(empty); - - // Marks the current goal as canceled. - active_goal_.setCanceled(); - has_active_goal_ = false; - } - else - { - ROS_WARN("Active goal and goal cancel do not match, ignoring cancel request"); - } -} - -void JointTrajectoryAction::controllerStateCB(const control_msgs::FollowJointTrajectoryFeedbackConstPtr &msg) -{ - //ROS_DEBUG("Checking controller state feedback"); - last_trajectory_state_ = msg; - trajectory_state_recvd_ = true; - - if (!has_active_goal_) - { - //ROS_DEBUG("No active goal, ignoring feedback"); - return; - } - if (current_traj_.points.empty()) - { - ROS_DEBUG("Current trajectory is empty, ignoring feedback"); - return; - } - - if (!industrial_utils::isSimilar(joint_names_, msg->joint_names)) - { - ROS_ERROR("Joint names from the controller don't match our joint names."); - return; - } - - // Checking for goal constraints - // Checks that we have ended inside the goal constraints and has motion stopped - - ROS_DEBUG("Checking goal constraints"); - if (withinGoalConstraints(last_trajectory_state_, current_traj_)) - { - if (last_robot_status_) - { - // Additional check for motion stoppage since the controller goal may still - // be moving. The current robot driver calls a motion stop if it receives - // a new trajectory while it is still moving. If the driver is not publishing - // the motion state (i.e. old driver), this will still work, but it warns you. - if (last_robot_status_->in_motion.val == industrial_msgs::TriState::FALSE) - { - ROS_INFO("Inside goal constraints, stopped moving, return success for action"); - active_goal_.setSucceeded(); - has_active_goal_ = false; - } - else if (last_robot_status_->in_motion.val == industrial_msgs::TriState::UNKNOWN) - { - ROS_INFO("Inside goal constraints, return success for action"); - ROS_WARN("Robot status in motion unknown, the robot driver node and controller code should be updated"); - active_goal_.setSucceeded(); - has_active_goal_ = false; - } - else - { - ROS_DEBUG("Within goal constraints but robot is still moving"); - } - } - else - { - ROS_INFO("Inside goal constraints, return success for action"); - ROS_WARN("Robot status is not being published the robot driver node and controller code should be updated"); - active_goal_.setSucceeded(); - has_active_goal_ = false; - } - } -} - -void JointTrajectoryAction::abortGoal() -{ - // Stops the controller. - trajectory_msgs::JointTrajectory empty; - pub_trajectory_command_.publish(empty); - - // Marks the current goal as aborted. - active_goal_.setAborted(); - has_active_goal_ = false; -} - -bool JointTrajectoryAction::withinGoalConstraints(const control_msgs::FollowJointTrajectoryFeedbackConstPtr &msg, - const trajectory_msgs::JointTrajectory & traj) -{ - bool rtn = false; - if (traj.points.empty()) - { - ROS_WARN("Empty joint trajectory passed to check goal constraints, return false"); - rtn = false; - } - else - { - int last_point = traj.points.size() - 1; - - if (industrial_robot_client::utils::isWithinRange(last_trajectory_state_->joint_names, - last_trajectory_state_->actual.positions, traj.joint_names, - traj.points[last_point].positions, goal_threshold_)) - { - rtn = true; - } - else - { - rtn = false; - } - } - return rtn; -} - -} //joint_trajectory_action -} //industrial_robot_client - diff --git a/training/supplements/src/industrial_core/industrial_robot_client/src/joint_trajectory_downloader.cpp b/training/supplements/src/industrial_core/industrial_robot_client/src/joint_trajectory_downloader.cpp deleted file mode 100644 index e03b180e..00000000 --- a/training/supplements/src/industrial_core/industrial_robot_client/src/joint_trajectory_downloader.cpp +++ /dev/null @@ -1,83 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2011, Southwest Research Institute - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in the - * documentation and/or other materials provided with the distribution. - * * Neither the name of the Southwest Research Institute, nor the names - * of its contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#include "industrial_robot_client/joint_trajectory_downloader.h" - -namespace industrial_robot_client -{ -namespace joint_trajectory_downloader -{ - -using industrial::simple_message::SimpleMessage; -namespace SpecialSeqValues = industrial::joint_traj_pt::SpecialSeqValues; - -bool JointTrajectoryDownloader::send_to_robot(const std::vector& messages) -{ - bool rslt=true; - std::vector points(messages); - SimpleMessage msg; - - // Trajectory download requires at least two points (START/END) - if (points.size() < 2) - points.push_back(JointTrajPtMessage(points[0])); - - // The first and last points are assigned special sequence values - points.begin()->setSequence(SpecialSeqValues::START_TRAJECTORY_DOWNLOAD); - points.back().setSequence(SpecialSeqValues::END_TRAJECTORY); - - if (!this->connection_->isConnected()) - { - ROS_WARN("Attempting robot reconnection"); - this->connection_->makeConnect(); - } - - ROS_INFO("Sending trajectory points, size: %d", (int)points.size()); - - for (int i = 0; i < (int)points.size(); ++i) - { - ROS_DEBUG("Sending joints trajectory point[%d]", i); - - points[i].toTopic(msg); - bool ptRslt = this->connection_->sendMsg(msg); - if (ptRslt) - ROS_DEBUG("Point[%d] sent to controller", i); - else - ROS_WARN("Failed sent joint point, skipping point"); - - rslt &= ptRslt; - } - - return rslt; -} - -} //joint_trajectory_downloader -} //industrial_robot_client - diff --git a/training/supplements/src/industrial_core/industrial_robot_client/src/joint_trajectory_interface.cpp b/training/supplements/src/industrial_core/industrial_robot_client/src/joint_trajectory_interface.cpp deleted file mode 100644 index f0ea3e2b..00000000 --- a/training/supplements/src/industrial_core/industrial_robot_client/src/joint_trajectory_interface.cpp +++ /dev/null @@ -1,374 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2011, Southwest Research Institute - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in the - * documentation and/or other materials provided with the distribution. - * * Neither the name of the Southwest Research Institute, nor the names - * of its contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#include -#include "industrial_robot_client/joint_trajectory_interface.h" -#include "simple_message/joint_traj_pt.h" -#include "industrial_utils/param_utils.h" - -using namespace industrial_utils::param; -using industrial::simple_message::SimpleMessage; -namespace StandardSocketPorts = industrial::simple_socket::StandardSocketPorts; -namespace SpecialSeqValues = industrial::joint_traj_pt::SpecialSeqValues; -typedef industrial::joint_traj_pt::JointTrajPt rbt_JointTrajPt; -typedef trajectory_msgs::JointTrajectoryPoint ros_JointTrajPt; - -namespace industrial_robot_client -{ -namespace joint_trajectory_interface -{ - -#define ROS_ERROR_RETURN(rtn,...) do {ROS_ERROR(__VA_ARGS__); return(rtn);} while(0) - -bool JointTrajectoryInterface::init(std::string default_ip, int default_port) -{ - std::string ip; - int port; - - // override IP/port with ROS params, if available - ros::param::param("robot_ip_address", ip, default_ip); - ros::param::param("~port", port, default_port); - - // check for valid parameter values - if (ip.empty()) - { - ROS_ERROR("No valid robot IP address found. Please set ROS 'robot_ip_address' param"); - return false; - } - if (port <= 0) - { - ROS_ERROR("No valid robot IP port found. Please set ROS '~port' param"); - return false; - } - - char* ip_addr = strdup(ip.c_str()); // connection.init() requires "char*", not "const char*" - ROS_INFO("Joint Trajectory Interface connecting to IP address: '%s:%d'", ip_addr, port); - default_tcp_connection_.init(ip_addr, port); - free(ip_addr); - - return init(&default_tcp_connection_); -} - -bool JointTrajectoryInterface::init(SmplMsgConnection* connection) -{ - std::vector joint_names; - if (!getJointNames("controller_joint_names", "robot_description", joint_names)) - { - ROS_ERROR("Failed to initialize joint_names. Aborting"); - return false; - } - - return init(connection, joint_names); -} - -bool JointTrajectoryInterface::init(SmplMsgConnection* connection, const std::vector &joint_names, - const std::map &velocity_limits) -{ - this->connection_ = connection; - this->all_joint_names_ = joint_names; - this->joint_vel_limits_ = velocity_limits; - connection_->makeConnect(); - - // try to read velocity limits from URDF, if none specified - if (joint_vel_limits_.empty() && !industrial_utils::param::getJointVelocityLimits("robot_description", joint_vel_limits_)) - ROS_WARN("Unable to read velocity limits from 'robot_description' param. Velocity validation disabled."); - - this->srv_stop_motion_ = this->node_.advertiseService("stop_motion", &JointTrajectoryInterface::stopMotionCB, this); - this->srv_joint_trajectory_ = this->node_.advertiseService("joint_path_command", &JointTrajectoryInterface::jointTrajectoryCB, this); - this->sub_joint_trajectory_ = this->node_.subscribe("joint_path_command", 0, &JointTrajectoryInterface::jointTrajectoryCB, this); - this->sub_cur_pos_ = this->node_.subscribe("joint_states", 1, &JointTrajectoryInterface::jointStateCB, this); - - return true; -} - -JointTrajectoryInterface::~JointTrajectoryInterface() -{ - trajectoryStop(); - this->sub_joint_trajectory_.shutdown(); -} - -bool JointTrajectoryInterface::jointTrajectoryCB(industrial_msgs::CmdJointTrajectory::Request &req, - industrial_msgs::CmdJointTrajectory::Response &res) -{ - trajectory_msgs::JointTrajectoryPtr traj_ptr(new trajectory_msgs::JointTrajectory); - *traj_ptr = req.trajectory; // copy message data - this->jointTrajectoryCB(traj_ptr); - - // no success/fail result from jointTrajectoryCB. Assume success. - res.code.val = industrial_msgs::ServiceReturnCode::SUCCESS; - - return true; // always return true. To distinguish between call-failed and service-unavailable. -} - -void JointTrajectoryInterface::jointTrajectoryCB(const trajectory_msgs::JointTrajectoryConstPtr &msg) -{ - ROS_INFO("Receiving joint trajectory message"); - - // check for STOP command - if (msg->points.empty()) - { - ROS_INFO("Empty trajectory received, canceling current trajectory"); - trajectoryStop(); - return; - } - - // convert trajectory into robot-format - std::vector robot_msgs; - if (!trajectory_to_msgs(msg, &robot_msgs)) - return; - - // send command messages to robot - send_to_robot(robot_msgs); -} - -bool JointTrajectoryInterface::trajectory_to_msgs(const trajectory_msgs::JointTrajectoryConstPtr& traj, std::vector* msgs) -{ - msgs->clear(); - - // check for valid trajectory - if (!is_valid(*traj)) - return false; - - for (size_t i=0; ipoints.size(); ++i) - { - ros_JointTrajPt rbt_pt, xform_pt; - double vel, duration; - - // select / reorder joints for sending to robot - if (!select(traj->joint_names, traj->points[i], this->all_joint_names_, &rbt_pt)) - return false; - - // transform point data (e.g. for joint-coupling) - if (!transform(rbt_pt, &xform_pt)) - return false; - - // reduce velocity to a single scalar, for robot command - if (!calc_speed(xform_pt, &vel, &duration)) - return false; - - JointTrajPtMessage msg = create_message(i, xform_pt.positions, vel, duration); - msgs->push_back(msg); - } - - return true; -} - -bool JointTrajectoryInterface::select(const std::vector& ros_joint_names, const ros_JointTrajPt& ros_pt, - const std::vector& rbt_joint_names, ros_JointTrajPt* rbt_pt) -{ - ROS_ASSERT(ros_joint_names.size() == ros_pt.positions.size()); - - // initialize rbt_pt - *rbt_pt = ros_pt; - rbt_pt->positions.clear(); rbt_pt->velocities.clear(); rbt_pt->accelerations.clear(); - - for (size_t rbt_idx=0; rbt_idx < rbt_joint_names.size(); ++rbt_idx) - { - bool is_empty = rbt_joint_names[rbt_idx].empty(); - - // find matching ROS element - size_t ros_idx = std::find(ros_joint_names.begin(), ros_joint_names.end(), rbt_joint_names[rbt_idx]) - ros_joint_names.begin(); - bool is_found = ros_idx < ros_joint_names.size(); - - // error-chk: required robot joint not found in ROS joint-list - if (!is_empty && !is_found) - { - ROS_ERROR("Expected joint (%s) not found in JointTrajectory. Aborting command.", rbt_joint_names[rbt_idx].c_str()); - return false; - } - - if (is_empty) - { - if (!ros_pt.positions.empty()) rbt_pt->positions.push_back(default_joint_pos_); - if (!ros_pt.velocities.empty()) rbt_pt->velocities.push_back(-1); - if (!ros_pt.accelerations.empty()) rbt_pt->accelerations.push_back(-1); - } - else - { - if (!ros_pt.positions.empty()) rbt_pt->positions.push_back(ros_pt.positions[ros_idx]); - if (!ros_pt.velocities.empty()) rbt_pt->velocities.push_back(ros_pt.velocities[ros_idx]); - if (!ros_pt.accelerations.empty()) rbt_pt->accelerations.push_back(ros_pt.accelerations[ros_idx]); - } - } - return true; -} - -bool JointTrajectoryInterface::calc_speed(const trajectory_msgs::JointTrajectoryPoint& pt, double* rbt_velocity, double* rbt_duration) -{ - return calc_velocity(pt, rbt_velocity) && calc_duration(pt, rbt_duration); -} - -// default velocity calculation computes the %-of-max-velocity for the "critical joint" (closest to velocity-limit) -// such that 0.2 = 20% of maximum joint speed. -// -// NOTE: this calculation uses the maximum joint speeds from the URDF file, which may differ from those defined on -// the physical robot. These differences could lead to different actual movement velocities than intended. -// Behavior should be verified on a physical robot if movement velocity is critical. -bool JointTrajectoryInterface::calc_velocity(const trajectory_msgs::JointTrajectoryPoint& pt, double* rbt_velocity) -{ - std::vector vel_ratios; - - ROS_ASSERT(all_joint_names_.size() == pt.positions.size()); - - // check for empty velocities in ROS topic - if (pt.velocities.empty()) - { - ROS_WARN("Joint velocities unspecified. Using default/safe speed."); - *rbt_velocity = default_vel_ratio_; - return true; - } - - for (size_t i=0; i 0) - *rbt_velocity = vel_ratios[max_idx]; - else - { - ROS_WARN_ONCE("Joint velocity-limits unspecified. Using default velocity-ratio."); - *rbt_velocity = default_vel_ratio_; - } - - if ( (*rbt_velocity < 0) || (*rbt_velocity > 1) ) - { - ROS_WARN("computed velocity (%.1f %%) is out-of-range. Clipping to [0-100%%]", *rbt_velocity * 100); - *rbt_velocity = std::min(1.0, std::max(0.0, *rbt_velocity)); // clip to [0,1] - } - - return true; -} - -bool JointTrajectoryInterface::calc_duration(const trajectory_msgs::JointTrajectoryPoint& pt, double* rbt_duration) -{ - std::vector durations; - double this_time = pt.time_from_start.toSec(); - static double last_time = 0; - - if (this_time <= last_time) // earlier time => new trajectory. Move slowly to first point. - *rbt_duration = default_duration_; - else - *rbt_duration = this_time - last_time; - - last_time = this_time; - - return true; -} - -JointTrajPtMessage JointTrajectoryInterface::create_message(int seq, std::vector joint_pos, double velocity, double duration) -{ - industrial::joint_data::JointData pos; - ROS_ASSERT(joint_pos.size() <= (unsigned int)pos.getMaxNumJoints()); - - for (size_t i=0; iconnection_->sendAndReceiveMsg(msg, reply); -} - -bool JointTrajectoryInterface::stopMotionCB(industrial_msgs::StopMotion::Request &req, - industrial_msgs::StopMotion::Response &res) -{ - trajectoryStop(); - - // no success/fail result from trajectoryStop. Assume success. - res.code.val = industrial_msgs::ServiceReturnCode::SUCCESS; - - return true; // always return true. To distinguish between call-failed and service-unavailable. -} - -bool JointTrajectoryInterface::is_valid(const trajectory_msgs::JointTrajectory &traj) -{ - for (int i=0; i::iterator max_vel = joint_vel_limits_.find(traj.joint_names[j]); - if (max_vel == joint_vel_limits_.end()) continue; // no velocity-checking if limit not defined - - if (std::abs(pt.velocities[j]) > max_vel->second) - ROS_ERROR_RETURN(false, "Validation failed: Max velocity exceeded for trajectory pt %d, joint '%s'", i, traj.joint_names[j].c_str()); - } - - // check for valid timestamp - if ((i > 0) && (pt.time_from_start.toSec() == 0)) - ROS_ERROR_RETURN(false, "Validation failed: Missing valid timestamp data for trajectory pt %d", i); - } - - return true; -} - -// copy robot JointState into local cache -void JointTrajectoryInterface::jointStateCB(const sensor_msgs::JointStateConstPtr &msg) -{ - this->cur_joint_pos_ = *msg; -} - -} //joint_trajectory_interface -} //industrial_robot_client - diff --git a/training/supplements/src/industrial_core/industrial_robot_client/src/joint_trajectory_streamer.cpp b/training/supplements/src/industrial_core/industrial_robot_client/src/joint_trajectory_streamer.cpp deleted file mode 100644 index abf7fe1c..00000000 --- a/training/supplements/src/industrial_core/industrial_robot_client/src/joint_trajectory_streamer.cpp +++ /dev/null @@ -1,224 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2011, Southwest Research Institute - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in the - * documentation and/or other materials provided with the distribution. - * * Neither the name of the Southwest Research Institute, nor the names - * of its contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#include "industrial_robot_client/joint_trajectory_streamer.h" - -using industrial::simple_message::SimpleMessage; - -namespace industrial_robot_client -{ -namespace joint_trajectory_streamer -{ - -bool JointTrajectoryStreamer::init(SmplMsgConnection* connection, const std::vector &joint_names, - const std::map &velocity_limits) -{ - bool rtn = true; - - ROS_INFO("JointTrajectoryStreamer: init"); - - rtn &= JointTrajectoryInterface::init(connection, joint_names, velocity_limits); - - this->mutex_.lock(); - this->current_point_ = 0; - this->state_ = TransferStates::IDLE; - this->streaming_thread_ = - new boost::thread(boost::bind(&JointTrajectoryStreamer::streamingThread, this)); - ROS_INFO("Unlocking mutex"); - this->mutex_.unlock(); - - return rtn; -} - -JointTrajectoryStreamer::~JointTrajectoryStreamer() -{ - delete this->streaming_thread_; -} - -void JointTrajectoryStreamer::jointTrajectoryCB(const trajectory_msgs::JointTrajectoryConstPtr &msg) -{ - ROS_INFO("Receiving joint trajectory message"); - - // read current state value (should be atomic) - int state = this->state_; - - ROS_DEBUG("Current state is: %d", state); - if (TransferStates::IDLE != state) - { - if (msg->points.empty()) - ROS_INFO("Empty trajectory received, canceling current trajectory"); - else - ROS_ERROR("Trajectory splicing not yet implemented, stopping current motion."); - - this->mutex_.lock(); - trajectoryStop(); - this->mutex_.unlock(); - return; - } - - if (msg->points.empty()) - { - ROS_INFO("Empty trajectory received while in IDLE state, nothing is done"); - return; - } - - // calc new trajectory - std::vector new_traj_msgs; - if (!trajectory_to_msgs(msg, &new_traj_msgs)) - return; - - // send command messages to robot - send_to_robot(new_traj_msgs); -} - -bool JointTrajectoryStreamer::send_to_robot(const std::vector& messages) -{ - ROS_INFO("Loading trajectory, setting state to streaming"); - this->mutex_.lock(); - { - ROS_INFO("Executing trajectory of size: %d", (int)messages.size()); - this->current_traj_ = messages; - this->current_point_ = 0; - this->state_ = TransferStates::STREAMING; - this->streaming_start_ = ros::Time::now(); - } - this->mutex_.unlock(); - - return true; -} - -bool JointTrajectoryStreamer::trajectory_to_msgs(const trajectory_msgs::JointTrajectoryConstPtr &traj, std::vector* msgs) -{ - // use base function to transform points - if (!JointTrajectoryInterface::trajectory_to_msgs(traj, msgs)) - return false; - - // pad trajectory as required for minimum streaming buffer size - if (!msgs->empty() && (msgs->size() < (size_t)min_buffer_size_)) - { - ROS_DEBUG("Padding trajectory: current(%d) => minimum(%d)", (int)msgs->size(), min_buffer_size_); - while (msgs->size() < (size_t)min_buffer_size_) - msgs->push_back(msgs->back()); - } - - return true; -} - - -void JointTrajectoryStreamer::streamingThread() -{ - JointTrajPtMessage jtpMsg; - int connectRetryCount = 1; - - ROS_INFO("Starting joint trajectory streamer thread"); - while (ros::ok()) - { - ros::Duration(0.005).sleep(); - - // automatically re-establish connection, if required - if (connectRetryCount-- > 0) - { - ROS_INFO("Connecting to robot motion server"); - this->connection_->makeConnect(); - ros::Duration(0.250).sleep(); // wait for connection - - if (this->connection_->isConnected()) - connectRetryCount = 0; - else if (connectRetryCount <= 0) - { - ROS_ERROR("Timeout connecting to robot controller. Send new motion command to retry."); - this->state_ = TransferStates::IDLE; - } - continue; - } - - this->mutex_.lock(); - - SimpleMessage msg, reply; - - switch (this->state_) - { - case TransferStates::IDLE: - ros::Duration(0.250).sleep(); // slower loop while waiting for new trajectory - break; - - case TransferStates::STREAMING: - if (this->current_point_ >= (int)this->current_traj_.size()) - { - ROS_INFO("Trajectory streaming complete, setting state to IDLE"); - this->state_ = TransferStates::IDLE; - break; - } - - if (!this->connection_->isConnected()) - { - ROS_DEBUG("Robot disconnected. Attempting reconnect..."); - connectRetryCount = 5; - break; - } - - jtpMsg = this->current_traj_[this->current_point_]; - jtpMsg.toRequest(msg); - - ROS_DEBUG("Sending joint trajectory point"); - if (this->connection_->sendAndReceiveMsg(msg, reply, false)) - { - ROS_INFO("Point[%d of %d] sent to controller", - this->current_point_, (int)this->current_traj_.size()); - this->current_point_++; - } - else - ROS_WARN("Failed sent joint point, will try again"); - - break; - default: - ROS_ERROR("Joint trajectory streamer: unknown state"); - this->state_ = TransferStates::IDLE; - break; - } - - this->mutex_.unlock(); - } - - ROS_WARN("Exiting trajectory streamer thread"); -} - -void JointTrajectoryStreamer::trajectoryStop() -{ - JointTrajectoryInterface::trajectoryStop(); - - ROS_DEBUG("Stop command sent, entering idle mode"); - this->state_ = TransferStates::IDLE; -} - -} //joint_trajectory_streamer -} //industrial_robot_client - diff --git a/training/supplements/src/industrial_core/industrial_robot_client/src/robot_state_interface.cpp b/training/supplements/src/industrial_core/industrial_robot_client/src/robot_state_interface.cpp deleted file mode 100644 index 9d05367d..00000000 --- a/training/supplements/src/industrial_core/industrial_robot_client/src/robot_state_interface.cpp +++ /dev/null @@ -1,120 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2011, Southwest Research Institute - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in the - * documentation and/or other materials provided with the distribution. - * * Neither the name of the Southwest Research Institute, nor the names - * of its contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#include "industrial_robot_client/robot_state_interface.h" -#include "industrial_utils/param_utils.h" - -using industrial::smpl_msg_connection::SmplMsgConnection; -using industrial_utils::param::getJointNames; -namespace StandardSocketPorts = industrial::simple_socket::StandardSocketPorts; - -namespace industrial_robot_client -{ -namespace robot_state_interface -{ - -RobotStateInterface::RobotStateInterface() -{ - this->connection_ = NULL; - this->add_handler(&default_joint_handler_); - this->add_handler(&default_robot_status_handler_); -} - -bool RobotStateInterface::init(std::string default_ip, int default_port) -{ - std::string ip; - int port; - - // override IP/port with ROS params, if available - ros::param::param("robot_ip_address", ip, default_ip); - ros::param::param("~port", port, default_port); - - // check for valid parameter values - if (ip.empty()) - { - ROS_ERROR("No valid robot IP address found. Please set ROS 'robot_ip_address' param"); - return false; - } - if (port <= 0) - { - ROS_ERROR("No valid robot IP port found. Please set ROS '~port' param"); - return false; - } - - char* ip_addr = strdup(ip.c_str()); // connection.init() requires "char*", not "const char*" - ROS_INFO("Robot state connecting to IP address: '%s:%d'", ip_addr, port); - default_tcp_connection_.init(ip_addr, port); - free(ip_addr); - - return init(&default_tcp_connection_); -} - -bool RobotStateInterface::init(SmplMsgConnection* connection) -{ - std::vector joint_names; - if (!getJointNames("controller_joint_names", "robot_description", joint_names)) - { - ROS_ERROR("Failed to initialize joint_names. Aborting"); - return false; - } - - return init(connection, joint_names); -} - -bool RobotStateInterface::init(SmplMsgConnection* connection, std::vector& joint_names) -{ - this->joint_names_ = joint_names; - this->connection_ = connection; - connection_->makeConnect(); - - // initialize message-manager - if (!manager_.init(connection_)) - return false; - - // initialize default handlers - if (!default_joint_handler_.init(connection_, joint_names_)) - return false; - this->add_handler(&default_joint_handler_); - - if (!default_robot_status_handler_.init(connection_)) - return false; - this->add_handler(&default_robot_status_handler_); - - return true; -} - -void RobotStateInterface::run() -{ - manager_.spin(); -} - -} // robot_state_interface -} // industrial_robot_client diff --git a/training/supplements/src/industrial_core/industrial_robot_client/src/robot_status_relay_handler.cpp b/training/supplements/src/industrial_core/industrial_robot_client/src/robot_status_relay_handler.cpp deleted file mode 100644 index 08130c8d..00000000 --- a/training/supplements/src/industrial_core/industrial_robot_client/src/robot_status_relay_handler.cpp +++ /dev/null @@ -1,94 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2012, Southwest Research Institute - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in the - * documentation and/or other materials provided with the distribution. - * * Neither the name of the Southwest Research Institute, nor the names - * of its contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#include "industrial_robot_client/robot_status_relay_handler.h" -#include "industrial_msgs/RobotStatus.h" -#include "simple_message/log_wrapper.h" - -using namespace industrial::shared_types; -using namespace industrial::smpl_msg_connection; -using namespace industrial::simple_message; -using namespace industrial::robot_status; -using namespace industrial::robot_status_message; - -namespace industrial_robot_client -{ -namespace robot_status_relay_handler -{ - -bool RobotStatusRelayHandler::init(SmplMsgConnection* connection) -{ - this->pub_robot_status_ = this->node_.advertise("robot_status", 1); - return init((int)StandardMsgTypes::STATUS, connection); -} - -bool RobotStatusRelayHandler::internalCB(SimpleMessage& in) -{ - RobotStatusMessage status_msg; - - if (!status_msg.init(in)) - { - LOG_ERROR("Failed to initialize status message"); - return false; - } - - return internalCB(status_msg); -} - -bool RobotStatusRelayHandler::internalCB(RobotStatusMessage & in) -{ - industrial_msgs::RobotStatus status; - bool rtn = true; - - status.drives_powered.val = TriStates::toROSMsgEnum(in.status_.getDrivesPowered()); - status.e_stopped.val = TriStates::toROSMsgEnum(in.status_.getEStopped()); - status.error_code = in.status_.getErrorCode(); - status.in_error.val = TriStates::toROSMsgEnum(in.status_.getInError()); - status.in_motion.val = TriStates::toROSMsgEnum(in.status_.getInMotion()); - status.mode.val = RobotModes::toROSMsgEnum(in.status_.getMode()); - status.motion_possible.val = TriStates::toROSMsgEnum(in.status_.getMotionPossible()); - - this->pub_robot_status_.publish(status); - - // Reply back to the controller if the sender requested it. - if (CommTypes::SERVICE_REQUEST == in.getMessageType()) - { - SimpleMessage reply; - in.toReply(reply, rtn ? ReplyTypes::SUCCESS : ReplyTypes::FAILURE); - this->getConnection()->sendMsg(reply); - } - - return rtn; -} - -} -} - diff --git a/training/supplements/src/industrial_core/industrial_robot_client/src/utils.cpp b/training/supplements/src/industrial_core/industrial_robot_client/src/utils.cpp deleted file mode 100644 index 2cd29892..00000000 --- a/training/supplements/src/industrial_core/industrial_robot_client/src/utils.cpp +++ /dev/null @@ -1,184 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2012, Southwest Research Institute - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in the - * documentation and/or other materials provided with the distribution. - * * Neither the name of the Southwest Research Institute, nor the names - * of its contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#include "ros/console.h" - -#include "industrial_utils/utils.h" -#include "industrial_robot_client/utils.h" - -#include -#include - -namespace industrial_robot_client -{ -namespace utils -{ - -bool isWithinRange(const std::vector & lhs, const std::vector & rhs, double full_range) -{ - bool rtn = false; - - if (lhs.size() != rhs.size()) - { - ROS_ERROR_STREAM(__FUNCTION__ << "::lhs size: " << lhs.size() << " does not match rhs size: " << rhs.size()); - rtn = false; - } - else - { - // Calculating the half range causes some precision loss, but it's good enough - double half_range = full_range / 2.0; - rtn = true; // Assume within range, catch exception in loop below - - // This loop will not run for empty vectors, results in return of true - for (size_t i = 0; i < lhs.size(); ++i) - { - if (fabs(lhs[i] - rhs[i]) > fabs(half_range)) - { - rtn = false; - break; - } - } - - } - - return rtn; -} - -bool mapInsert(const std::string & key, double value, std::map & mappings) -{ - bool rtn = false; - - std::pair::iterator, bool> insert_rtn; - - insert_rtn = mappings.insert(std::make_pair(key, value)); - - // The second value returned form insert is a boolean (true for success) - if (!insert_rtn.second) - { - ROS_ERROR_STREAM(__FUNCTION__ << "::Failed to insert item into map with key: " << key); - rtn = false; - } - else - { - rtn = true; - } - return rtn; - -} - -bool toMap(const std::vector & keys, const std::vector & values, - std::map & mappings) -{ - bool rtn; - - mappings.clear(); - - if (keys.size() == values.size()) - { - rtn = true; - - for (size_t i = 0; i < keys.size(); ++i) - { - rtn = mapInsert(keys[i], values[i], mappings); - if (!rtn) - { - break; - } - } - - } - else - { - ROS_ERROR_STREAM(__FUNCTION__ << "::keys size: " << keys.size() - << " does not match values size: " << values.size()); - - rtn = false; - } - - return rtn; -} - -bool isWithinRange(const std::vector & keys, const std::map & lhs, - const std::map & rhs, double full_range) -{ - bool rtn = false; - - if ((keys.size() != rhs.size()) || (keys.size() != lhs.size())) - { - ROS_ERROR_STREAM(__FUNCTION__ << "::Size mistmatch ::lhs size: " << lhs.size() << - " rhs size: " << rhs.size() << " key size: " << keys.size()); - - rtn = false; - } - else - { - // Calculating the half range causes some precision loss, but it's good enough - double half_range = full_range / 2.0; - rtn = true; // Assume within range, catch exception in loop below - - // This loop will not run for empty vectors, results in return of true - for (size_t i = 0; i < keys.size(); ++i) - { - if (fabs(lhs.at(keys[i]) - rhs.at(keys[i])) > fabs(half_range)) - { - rtn = false; - break; - } - } - - } - - return rtn; -} - -bool isWithinRange(const std::vector & lhs_keys, const std::vector & lhs_values, - const std::vector & rhs_keys, const std::vector & rhs_values, double full_range) -{ - bool rtn = false; - std::map lhs_map; - std::map rhs_map; - if (industrial_utils::isSimilar(lhs_keys, rhs_keys)) - { - if (toMap(lhs_keys, lhs_values, lhs_map) && toMap(rhs_keys, rhs_values, rhs_map)) - { - rtn = isWithinRange(lhs_keys, lhs_map, rhs_map, full_range); - } - } - else - { - ROS_ERROR_STREAM(__FUNCTION__ << "::Key vectors are not similar"); - rtn = false; - } - return rtn; -} - -} //utils -} //industrial_robot_client diff --git a/training/supplements/src/industrial_core/industrial_robot_client/test/utest.cpp b/training/supplements/src/industrial_core/industrial_robot_client/test/utest.cpp deleted file mode 100644 index b0a4f7d6..00000000 --- a/training/supplements/src/industrial_core/industrial_robot_client/test/utest.cpp +++ /dev/null @@ -1,144 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2012, Southwest Research Institute - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in the - * documentation and/or other materials provided with the distribution. - * * Neither the name of the Southwest Research Institute, nor the names - * of its contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#include "industrial_robot_client/utils.h" -#include -#include - -using namespace industrial_robot_client::utils; - - -TEST(IndustrialUtilsSuite, vector_within_range) -{ - - std::vector v1; - std::vector v2; - const double range = 0.01; - const double half_range = range / 2.0; - const double outside_range = 1.1 * half_range; - const double inside_range = 0.9 * half_range; - const double a = 9.65; - const double b = 1324.239; - const double c = 90204987243.1433223; - - //NOTE: Asserts are used because each step builds upon the the prevoius step - EXPECT_TRUE(isWithinRange(v1, v2, range)); - - v1.push_back(a); - ASSERT_FALSE(isWithinRange(v1, v2, range)); - - v2.push_back(a); - ASSERT_TRUE(isWithinRange(v1, v2, range)); - ASSERT_TRUE(isWithinRange(v1, v2, -range)); - - v1.push_back(b); - ASSERT_FALSE(isWithinRange(v1, v2, range)); - - v2.push_back(b + inside_range); - ASSERT_TRUE(isWithinRange(v1, v2, range)); - - v1.push_back(b); - v2.push_back(b + outside_range); - ASSERT_FALSE(isWithinRange(v1, v2, range)); - - v1.pop_back(); - v2.pop_back(); - - v1.push_back(c); - v2.push_back(c + inside_range); - ASSERT_TRUE(isWithinRange(v1, v2, range)); - - v1.push_back(c); - v2.push_back(c + outside_range); - ASSERT_FALSE(isWithinRange(v1, v2, range)); - -} - -TEST(IndustrialUtilsSuite, map_within_range) -{ - std::vector v1; - std::vector v1_keys; - std::vector v2; - std::vector v2_keys; - std::vector v3; - std::vector v3_keys; - const double range = 0.01; - const double half_range = range / 2.0; - const double outside_range = 1.1 * half_range; - const double inside_range = 0.9 * half_range; - const double a = 9.65; - const double b = 1324.239; - const double c = 90204987243.1433223; - - - // Empty vectors should return true - EXPECT_TRUE(isWithinRange(v1_keys, v1, v2_keys, v2, range)); - - - v1.push_back(a); - v1_keys.push_back("a"); - v1.push_back(b); - v1_keys.push_back("b"); - v1.push_back(c); - v1_keys.push_back("c"); - - ASSERT_FALSE(isWithinRange(v1_keys, v1, v2_keys, v2, range)); - EXPECT_TRUE(isWithinRange(v1_keys, v1, v1_keys, v1, range)); - - - v2.push_back(inside_range + c); - v2_keys.push_back("c"); - v2.push_back(inside_range + b); - v2_keys.push_back("b"); - v2.push_back(inside_range + a); - v2_keys.push_back("a"); - - ASSERT_TRUE(isWithinRange(v1_keys, v1, v2_keys, v2, range)); - - v3.push_back(outside_range + a); - v3_keys.push_back("a"); - v3.push_back(outside_range + b); - v3_keys.push_back("b"); - v3.push_back(outside_range + c); - v3_keys.push_back("c"); - - - ASSERT_FALSE(isWithinRange(v1_keys, v1, v3_keys, v3, range)); - - -} - -// Run all the tests that were declared with TEST() - int main(int argc, char **argv) - { - testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); - } diff --git a/training/supplements/src/industrial_core/industrial_robot_simulator/CHANGELOG.rst b/training/supplements/src/industrial_core/industrial_robot_simulator/CHANGELOG.rst deleted file mode 100644 index 452e9e9a..00000000 --- a/training/supplements/src/industrial_core/industrial_robot_simulator/CHANGELOG.rst +++ /dev/null @@ -1,23 +0,0 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package industrial_robot_simulator -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -0.3.3 (2014-01-13) ------------------- -* No change -0.3.2 (2014-01-10) ------------------- -* Removed header from industrial_utils/utils.h (not required) - -0.3.1 (2014-01-09) ------------------- -* robot_simulator: avoid hardcoded python path. Fix `#53 `_. -* Remove obsolete export tags. Fix `#43 `_. -* Add install target for launchfile in sim pkg. - Fix `#35 `_. -* Fix issue `#6 `_ (Install target fails for industrial_robot_simulator): setup.py file not required for python executables, see http://ros.org/wiki/catkin/migrating_from_rosbuild -* bugFix: `#61 `_ - fix joint-name remapping in industrial_robot_simulator -* Added interpolated motion to MotionControllerSimulator class. Includes addition of interpolate(), and modification of _motion_worker() -* New rospy.get_param() added to IndustrialRobotSimulatorNode in order to assign motion_update_rate -* Converted to catkin -* Contributors: JeremyZoss, Shaun Edwards, dpsolomon, gavanderhoorn, jrgnicho diff --git a/training/supplements/src/industrial_core/industrial_robot_simulator/CMakeLists.txt b/training/supplements/src/industrial_core/industrial_robot_simulator/CMakeLists.txt deleted file mode 100644 index cc4fc63c..00000000 --- a/training/supplements/src/industrial_core/industrial_robot_simulator/CMakeLists.txt +++ /dev/null @@ -1,29 +0,0 @@ -cmake_minimum_required(VERSION 2.8.3) - -project(industrial_robot_simulator) - -find_package(catkin REQUIRED COMPONENTS roscpp std_msgs sensor_msgs - control_msgs trajectory_msgs industrial_robot_client) - -catkin_package( - CATKIN_DEPENDS roscpp std_msgs sensor_msgs control_msgs trajectory_msgs industrial_robot_client -) - -########## -## Test ## -########## - -# ROS launch testing -## ROS launch testing should be re-enabled when the roslaunch script is officially -## released (see https://github.com/ros/ros_comm/pull/333) -##find_package(roslaunch) -##roslaunch_add_file_check(launch) - -############# -## Install ## -############# - - -install(PROGRAMS industrial_robot_simulator DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) - -install(DIRECTORY launch/ DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch) diff --git a/training/supplements/src/industrial_core/industrial_robot_simulator/industrial_robot_simulator b/training/supplements/src/industrial_core/industrial_robot_simulator/industrial_robot_simulator deleted file mode 100755 index ebc9519c..00000000 --- a/training/supplements/src/industrial_core/industrial_robot_simulator/industrial_robot_simulator +++ /dev/null @@ -1,376 +0,0 @@ -#!/usr/bin/env python - -import roslib; roslib.load_manifest('industrial_robot_simulator') -import rospy -import copy -import threading -import Queue - -# Publish -from sensor_msgs.msg import JointState -from control_msgs.msg import FollowJointTrajectoryFeedback - -# Subscribe -from trajectory_msgs.msg import JointTrajectory -from trajectory_msgs.msg import JointTrajectoryPoint - -# Reference -from industrial_msgs.msg import TriState -import trajectory_msgs - - - - - -""" -MotionControllerSimulator - -This class simulates the motion controller for an industrial robot. - -This class IS threadsafe - -""" -class MotionControllerSimulator(): - - """ - Constructor of motion controller simulator - """ - def __init__(self, num_joints, initial_joint_state, update_rate = 100, buffer_size = 0): - - # Class lock - self.lock = threading.Lock() - - # Motion loop update rate (higher update rates result in smoother simulated motion) - self.update_rate = update_rate - rospy.loginfo("Setting motion update rate (hz): %f", self.update_rate) - - # Initialize joint position - self.joint_positions = initial_joint_state - rospy.loginfo("Setting initial joint state: %s", str(initial_joint_state)) - - # Initialize motion buffer (contains joint position lists) - self.motion_buffer = Queue.Queue(buffer_size) - rospy.loginfo("Setting motion buffer size: %i", buffer_size) - - # Shutdown signal - self.sig_shutdown = False - - # Stop signal - self.sig_stop = False - - # Motion thread - self.motion_thread = threading.Thread(target=self._motion_worker) - self.motion_thread.daemon = True - self.motion_thread.start() - - - """ - """ - def add_motion_waypoint(self, point): - - self.motion_buffer.put(point) - - - """ - """ - def get_joint_positions(self): - - with self.lock: - return self.joint_positions[:] - - """ - """ - def is_in_motion(self): - return not self.motion_buffer.empty() - - """ - """ - def shutdown(self): - self.sig_shutdown = True - rospy.loginfo('Motion_Controller shutdown signaled') - - """ - """ - def stop(self): - rospy.loginfo('Motion_Controller stop signaled') - with self.lock: - self._clear_buffer() - self.sig_stop = True - - """ - """ - def interpolate(self, last_pt, current_pt, alpha): - intermediate_pt = JointTrajectoryPoint() - for last_joint, current_joint in zip(last_pt.positions, current_pt.positions): - intermediate_pt.positions.append(last_joint + alpha*(current_joint-last_joint)) - intermediate_pt.time_from_start = last_pt.time_from_start + rospy.Duration(alpha*(current_pt.time_from_start.to_sec() - last_pt.time_from_start.to_sec())) - return intermediate_pt - - """ - """ - def _clear_buffer(self): - with self.motion_buffer.mutex: - self.motion_buffer.queue.clear() - - """ - """ - def _move_to(self, point, dur): - - rospy.sleep(dur) - - with self.lock: - if not self.sig_stop: - self.joint_positions = point.positions[:] - #rospy.loginfo('Moved to position: %s in %s', str(self.joint_positions), str(dur)) - else: - rospy.loginfo('Stoping motion immediately, clearing stop signal') - self.sig_stop = False - - """ - """ - def _motion_worker(self): - - rospy.loginfo('Starting motion worker in motion controller simulator') - move_duration = rospy.Duration() - if self.update_rate <> 0.: - update_duration = rospy.Duration(1./self.update_rate) - last_goal_point = JointTrajectoryPoint() - - with self.lock: - last_goal_point.positions = self.joint_positions[:] - - while not self.sig_shutdown: - - try: - - current_goal_point = self.motion_buffer.get() - - # If the current time from start is less than the last, then it's a new trajectory - if current_goal_point.time_from_start < last_goal_point.time_from_start: - move_duration = current_goal_point.time_from_start - # Else it's an existing trajectory and subtract the two - else: - # If current move duration is greater than update_duration, move arm to interpolated joint position - # Provide an exception to this rule: if update rate is <=0, do not add interpolated points - move_duration = current_goal_point.time_from_start - last_goal_point.time_from_start - if self.update_rate > 0: - while update_duration < move_duration: - intermediate_goal_point = self.interpolate(last_goal_point, current_goal_point, update_duration.to_sec()/move_duration.to_sec()) - self._move_to(intermediate_goal_point, update_duration.to_sec()) #TODO should this use min(update_duration, 0.5*move_duration) to smooth timing? - last_goal_point = copy.deepcopy(intermediate_goal_point) - move_duration = current_goal_point.time_from_start - intermediate_goal_point.time_from_start - - self._move_to(current_goal_point, move_duration) - - last_goal_point = copy.deepcopy(current_goal_point) - - except Exception as e: - rospy.logerr('Unexpected exception: %s', e) - - - rospy.loginfo("Shutting down motion controller") - - - - -""" -IndustrialRobotSimulator - -This class simulates an industrial robot controller. The simulator -adheres to the ROS-Industrial robot driver specification: - -http://www.ros.org/wiki/Industrial/Industrial_Robot_Driver_Spec - -TODO: Currently the simulator only supports the bare minimum motion -interface. - -TODO: Interfaces to add: -Robot status -Joint streaming -All services - -""" -class IndustrialRobotSimulatorNode(): - - - """ - Constructor of industrial robot simulator - """ - def __init__(self): - rospy.init_node('industrial_robot_simulator') - - # Class lock - self.lock = threading.Lock() - - # Publish rate (hz) - self.pub_rate = rospy.get_param('pub_rate', 10.0) - rospy.loginfo("Setting publish rate (hz) based on parameter: %f", self.pub_rate) - - # Joint names - def_joint_names = ["joint_1", "joint_2", "joint_3", "joint_4", "joint_5", "joint_6"] - self.joint_names = rospy.get_param('controller_joint_names', def_joint_names) - if len(self.joint_names) == 0: - rospy.logwarn("Joint list is empty, did you set controller_joint_name?") - rospy.loginfo("Setting joint names based on parameter: %s", str(self.joint_names)) - - # Setup initial joint positions - num_joints = len(self.joint_names) - initial_joint_state = rospy.get_param('initial_joint_state', [0]*num_joints) - same_len = len(initial_joint_state) == num_joints - all_num = all(type(x) is int or type(x) is float for x in initial_joint_state) - if not same_len or not all_num: - initial_joint_state = [0]*num_joints - rospy.logwarn("Invalid initial_joint_state parameter, defaulting to all-zeros " - "(len: %s, types: %s).", same_len, all_num) - - # retrieve update rate - motion_update_rate = rospy.get_param('motion_update_rate', 100.); #set param to 0 to ignore interpolated motion - self.motion_ctrl = MotionControllerSimulator(num_joints, initial_joint_state, update_rate = motion_update_rate) - - # Published to joint states - rospy.loginfo("Creating joint state publisher") - self.joint_state_pub = rospy.Publisher('joint_states', JointState) - - # Published to joint feedback - rospy.loginfo("Creating joint feedback publisher") - self.joint_feedback_pub = rospy.Publisher('feedback_states', FollowJointTrajectoryFeedback) - - # Subscribe to a joint trajectory - rospy.loginfo("Creating joint trajectory subscriber") - self.joint_path_sub = rospy.Subscriber('joint_path_command', JointTrajectory, self.trajectory_callback) - - # Timed task (started automatically) - period = rospy.Duration(1.0/self.pub_rate) - rospy.loginfo('Setting up publish worker with period (sec): %s', str(period.to_sec())) - rospy.Timer(period, self.publish_worker) - - # Clean up init - rospy.on_shutdown(self.motion_ctrl.shutdown) - - - - - """ - The publish worker is executed at a fixed rate. The publishes the various - state and status information for the robot. - """ - def publish_worker(self, event): - self.joint_state_publisher() - - - """ - The joint state publisher publishes the current joint state and the current - feedback state (as these are closely releated) - """ - def joint_state_publisher(self): - - - try: - - joint_state_msg = JointState() - joint_fb_msg = FollowJointTrajectoryFeedback() - time = rospy.Time.now() - - with self.lock: - - #Joint states - - joint_state_msg.header.stamp = time - joint_state_msg.name = self.joint_names - joint_state_msg.position = self.motion_ctrl.get_joint_positions() - - self.joint_state_pub.publish(joint_state_msg) - - #Joint feedback - joint_fb_msg.header.stamp = time - joint_fb_msg.joint_names = self.joint_names - joint_fb_msg.actual.positions = self.motion_ctrl.get_joint_positions() - - self.joint_feedback_pub.publish(joint_fb_msg) - - except Exception as e: - rospy.logerr('Unexpected exception in joint state publisher: %s', e) - - - """ - Trajectory subscription callback (gets called whenever a joint trajectory - is received). - - @param msg_in: joint trajectory message - @type msg_in: JointTrajectory - """ - def trajectory_callback(self, msg_in): - - try: - - rospy.loginfo('Received trajectory with %s points, executing callback', str(len(msg_in.points))) - - if self.motion_ctrl.is_in_motion(): - if len(msg_in.points) > 0: - rospy.logerr('Received trajectory while still in motion, trajectory splicing not supported') - else: - rospy.loginfo('Received empty trajectory while still in motion, stopping current trajectory') - self.motion_ctrl.stop() - - else: - - for point in msg_in.points: - - point = self._to_controller_order(msg_in.joint_names, point) - self.motion_ctrl.add_motion_waypoint(point) - - except Exception as e: - rospy.logerr('Unexpected exception: %s', e) - - - rospy.loginfo('Exiting trajectory callback') - - - """ - Remaps point to controller joint order - - @param keys: keys defining joint value order - @type keys: list - @param point: joint trajectory point - @type point: JointTrajectoryPoint - - @return point: reorder point - @type point: JointTrajectoryPoint - """ - def _to_controller_order(self, keys, point): - - #rospy.loginfo('to controller order, keys: %s, point: %s', str(keys), str(point)) - pt_rtn = copy.deepcopy(point) - pt_rtn.positions = self._remap_order(self.joint_names, keys, point.positions) - - return pt_rtn - - def _remap_order(self, ordered_keys, value_keys, values): - - #rospy.loginfo('remap order, ordered_keys: %s, value_keys: %s, values: %s', str(ordered_keys), str(value_keys), str(values)) - ordered_values = [] - - - ordered_values = [0]*len(ordered_keys) - mapping = dict(zip(value_keys, values)) - #rospy.loginfo('maping: %s', str(mapping)) - - for i in range(len(ordered_keys)): - ordered_values[i] = mapping[ordered_keys[i]] - pass - - return ordered_values - - - - - - -if __name__ == '__main__': - try: - rospy.loginfo('Executing joint_controller_simulator') - controller = IndustrialRobotSimulatorNode() - rospy.spin() - except rospy.ROSInterruptException: pass - - diff --git a/training/supplements/src/industrial_core/industrial_robot_simulator/launch/robot_interface_simulator.launch b/training/supplements/src/industrial_core/industrial_robot_simulator/launch/robot_interface_simulator.launch deleted file mode 100644 index 438450a7..00000000 --- a/training/supplements/src/industrial_core/industrial_robot_simulator/launch/robot_interface_simulator.launch +++ /dev/null @@ -1,21 +0,0 @@ - - - - - - - - - - - - diff --git a/training/supplements/src/industrial_core/industrial_robot_simulator/mainpage.dox b/training/supplements/src/industrial_core/industrial_robot_simulator/mainpage.dox deleted file mode 100644 index 52f6d620..00000000 --- a/training/supplements/src/industrial_core/industrial_robot_simulator/mainpage.dox +++ /dev/null @@ -1,14 +0,0 @@ -/** -\mainpage -\htmlinclude manifest.html - -\b The industrial robot simulator is a stand in for industrial robot driver node(s). It adheres to the driver specification for industrial robot controllers. - - - ---> - - -*/ diff --git a/training/supplements/src/industrial_core/industrial_robot_simulator/package.xml b/training/supplements/src/industrial_core/industrial_robot_simulator/package.xml deleted file mode 100644 index 6d721fd0..00000000 --- a/training/supplements/src/industrial_core/industrial_robot_simulator/package.xml +++ /dev/null @@ -1,24 +0,0 @@ - - industrial_robot_simulator - 0.3.3 - The industrial robot simulator is a stand in for industrial robot driver node(s). It adheres to the driver specification for industrial robot controllers. - - Shaun Edwards - BSD - http://ros.org/wiki/industrial_robot_client - Shaun Edwards - - catkin - roscpp - std_msgs - sensor_msgs - control_msgs - trajectory_msgs - industrial_robot_client - roscpp - std_msgs - sensor_msgs - control_msgs - trajectory_msgs - industrial_robot_client - diff --git a/training/supplements/src/industrial_core/industrial_trajectory_filters/CHANGELOG.rst b/training/supplements/src/industrial_core/industrial_trajectory_filters/CHANGELOG.rst deleted file mode 100644 index 4822074d..00000000 --- a/training/supplements/src/industrial_core/industrial_trajectory_filters/CHANGELOG.rst +++ /dev/null @@ -1,17 +0,0 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package industrial_trajectory_filters -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -0.3.3 (2014-01-13) ------------------- -* No change -0.3.2 (2014-01-10) ------------------- -* Removed header from industrial_utils/utils.h (not required) - -0.3.1 (2014-01-09) ------------------- -* Initial release -* Imported industrial trajectory filters package from industrial experimental -* Converted to catkin -* Contributors: Shaun Edwards, gavanderhoorn diff --git a/training/supplements/src/industrial_core/industrial_trajectory_filters/CMakeLists.txt b/training/supplements/src/industrial_core/industrial_trajectory_filters/CMakeLists.txt deleted file mode 100644 index 88d77a3d..00000000 --- a/training/supplements/src/industrial_core/industrial_trajectory_filters/CMakeLists.txt +++ /dev/null @@ -1,29 +0,0 @@ -cmake_minimum_required(VERSION 2.8.3) - -project(industrial_trajectory_filters) - -find_package(catkin REQUIRED COMPONENTS moveit_ros_planning trajectory_msgs) - -catkin_package( - CATKIN_DEPENDS moveit_ros_planning trajectory_msgs - INCLUDE_DIRS include - LIBRARIES ${PROJECT_NAME} -) - -include_directories(include - ${catkin_INCLUDE_DIRS} -) - - -add_library(${PROJECT_NAME} src/n_point_filter.cpp src/uniform_sample_filter.cpp) -target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES}) - - -install(TARGETS ${PROJECT_NAME} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) - -install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}) - -install(FILES planning_request_adapters_plugin_description.xml - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) diff --git a/training/supplements/src/industrial_core/industrial_trajectory_filters/include/industrial_trajectory_filters/filter_base.h b/training/supplements/src/industrial_core/industrial_trajectory_filters/include/industrial_trajectory_filters/filter_base.h deleted file mode 100644 index 5591d337..00000000 --- a/training/supplements/src/industrial_core/industrial_trajectory_filters/include/industrial_trajectory_filters/filter_base.h +++ /dev/null @@ -1,272 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2011, Southwest Research Institute - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in the - * documentation and/or other materials provided with the distribution. - * * Neither the name of the Southwest Research Institute, nor the names - * of its contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef FILTER_BASE_H_ -#define FILTER_BASE_H_ - -#include -#include "ros/assert.h" -#include "ros/console.h" -#include "ros/ros.h" -#include -#include - -namespace industrial_trajectory_filters -{ - -/** - * @brief Message Adapter structure serves as a stand-in for the arm navigation message type with - * which the filter was specialized. - * - */ -struct MessageAdapter -{ - - /** - * @brief Arm navigation message type for trajectory filter - * - */ - struct Request - { - trajectory_msgs::JointTrajectory trajectory; - } request; -}; - -/** - * @brief This version of the FilterBase can be used to encapsulate the functionality from an arm navigation - * trajectory filter into a PlanningRequestAdapter that is used by MoveIt. - * - * Since this version of the FilterBase class only provides some degree of backwards compatibility - * with the original FilterBase interface, then some parts of the arm navigation filter must be modified in order - * to accommodate the differences introduced by this new interface class. - * - * The purely virtual methods "update" and "configure" have been preserved since it is assumed that any - * trajectory filter specialization implemented at least these two methods. The getter methods for the - * filter's type and name have been kept as well, however the corresponding members aren't populated from within - * the scope of the FilterBase class. Thus, the users implementation must fill these members with - * the appropriate string data. - * - * All the "getParam" methods for parameter lookup have been removed and so the filter implementation - * must be modified to read parameters using the regular roscpp parameter API (either through the node - * handle or the bare version) - * - * Steps for conversion to a Moveit PlanningRequestAdapter: - * - * a - Remove all arm_navigation header files since these can not be included in a catkin based package. - * - * b - Remove all header files that belong to a rosbuild only package. - * - * c - Include the #include " header file in your filter's - * implementation header file. - * - * d- In your filter implementation, replace the base class "filters::FilterBase" with - * "industrial_trajectory_filters::FilterBase" - * - * e- Remove all calls to the getParam() methods from the old FilterBase class and use the roscpp - * parameter interface for any parameter lookup tasks instead. - * - * f- In your filter's header file, declare a typedef that specializes your filter template implementation - * to a version that uses the "MessageAdapter" structure. For instance, if we are converting the NPointFilter - * filter class then we would add the following after the class declaration: - * - * typedef NPointFilter NPointFilterAdapter; - * - * In case the "MessageAdapter" structure does not fit the expected template class then the filter's implementation - * must provide its own adapter structure that resembles the necessary parts of expected arm_navigation message type. - * The specialization of such filter would then pass the custom adapter structure in the template argument as follows: - * - * typedef NPointFilter NPointFilterAdapter; - * - * g- In your filter's source file, remove the plugin declaration "PLUGINLIB_DECLARE_CLASS" and add the - * class loader macro. In the case of the "NPointFilter" this would be as follows: - * - * CLASS_LOADER_REGISTER_CLASS(industrial_trajectory_filters::NPointFilterAdapter, - * planning_request_adapter::PlanningRequestAdapter); - * - * h - All parameters used by your filter must me made available as a regular ros parameter. For instance, in the - * "ompl_planning_pipeline.launch" file in a moveit package for your robot you would add the following element: - * - * - * i - (Optional) the "adaptAndPlan" methods default implementation already maps the trajectory data between the - * MessageAdapter structure and the planning interface objects in the argument list. The filter's implementation - * should override this method whenever a custom adapter structure is used. - */ -template - class FilterBase : public planning_request_adapter::PlanningRequestAdapter - { - public: - - /** - * @brief Default constructor - */ - FilterBase() : - planning_request_adapter::PlanningRequestAdapter(), nh_("~"), configured_(false), filter_type_("FilterBase"), filter_name_( - "Unimplemented") - { - - } - - /** - * Default destructor - */ - virtual ~FilterBase() - { - } - ; - - /** - * @brief Update the filter and return the data separately. This function - * must be implemented in the derived class. - * - * @param data_in A reference to the data to be input to the filter - * @param data_out A reference to the data output location - * @return true on success, otherwise false. - */ - virtual bool update(const T& data_in, T& data_out)=0; - - /** - * @brief Original FilterBase method, return filter type - * @return filter type (as string) - */ - std::string getType() - { - return filter_type_; - } - ; - - /** - * @brief Original FitlerBase Method - * @return filter name (as string). - */ - inline const std::string& getName() - { - return filter_name_; - } - ; // original FilterBase method - - protected: - - /** - * @brief FilterBase method for the sub class to configure the filter - * This function must be implemented in the derived class. - * @return true if successful, otherwise false. - */ - virtual bool configure()=0; - - /** - * @brief filter name - */ - std::string filter_name_; - - /** - * The type of the filter (Used by FilterChain for Factory construction) - */ - std::string filter_type_; - - /** - * @brief Holds filter configuration state. - */ - bool configured_; - - /** - * @brief Internal node handle (used for parameter lookup) - */ - ros::NodeHandle nh_; - - protected: - - /** - * @brief Moveit Planning Request Adapter method. This basic implementation of the - * adaptAndPlan method calls the planner and then maps the trajectory data from the - * MotionPlanResponse object into the MessageAdapter mapping structure. The - * MessageAdapter object is then passed to the "update" method that resembles that - * from the old FilterBase interface class. The filtered trajectory is finally - * saved in the MotionPlanResponse object. - */ - virtual bool adaptAndPlan(const PlannerFn &planner, const planning_scene::PlanningSceneConstPtr &planning_scene, - const planning_interface::MotionPlanRequest &req, - planning_interface::MotionPlanResponse &res, - std::vector &added_path_index) const - { - - // non const pointer to this - FilterBase *p = const_cast*>(this); - - // calling the configure method - if (!configured_ && p->configure()) - { - p->configured_ = true; - } - - // moveit messages for saving trajectory data - moveit_msgs::RobotTrajectory robot_trajectory_in, robot_trajectory_out; - MessageAdapter trajectory_in, trajectory_out; // mapping structures - - // calling planner first - bool result = planner(planning_scene, req, res); - - // applying filter to planned trajectory - if (result && res.trajectory_) - { - // mapping arguments into message adapter struct - res.trajectory_->getRobotTrajectoryMsg(robot_trajectory_in); - trajectory_in.request.trajectory = robot_trajectory_in.joint_trajectory; - - // applying arm navigation filter to planned trajectory - p->update(trajectory_in, trajectory_out); - - // saving filtered trajectory into moveit message. - robot_trajectory_out.joint_trajectory = trajectory_out.request.trajectory; - res.trajectory_->setRobotTrajectoryMsg(planning_scene->getCurrentState(), robot_trajectory_out); - - } - - return result; - } - - /** - * @brief Return description string - * @return description (as a string) - */ - virtual std::string getDescription() const - { - // non const pointer to this - FilterBase *p = const_cast*>(this); - - std::stringstream ss; - ss << "Trajectory filter '" << p->getName() << "' of type '" << p->getType() << "'"; - return ss.str(); - } - }; - -} - -#endif /* FILTER_BASE_H_ */ diff --git a/training/supplements/src/industrial_core/industrial_trajectory_filters/include/industrial_trajectory_filters/n_point_filter.h b/training/supplements/src/industrial_core/industrial_trajectory_filters/include/industrial_trajectory_filters/n_point_filter.h deleted file mode 100644 index 612d6597..00000000 --- a/training/supplements/src/industrial_core/industrial_trajectory_filters/include/industrial_trajectory_filters/n_point_filter.h +++ /dev/null @@ -1,89 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2011, Southwest Research Institute - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in the - * documentation and/or other materials provided with the distribution. - * * Neither the name of the Southwest Research Institute, nor the names - * of its contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef N_POINT_FILTER_H_ -#define N_POINT_FILTER_H_ - -#include - -namespace industrial_trajectory_filters -{ - -/** - * @brief This is a simple filter which reduces a trajectory to N points or less - */ -template - - class NPointFilter : public industrial_trajectory_filters::FilterBase - { - public: - /** - * @brief Default constructor - */ - NPointFilter(); - /** - * @brief Default destructor - */ - ~NPointFilter(); - ; - - virtual bool configure(); - - // \brief Reduces a trajectory to N points or less. The resulting trajectory - // contains only point within the original trajectory (no interpolation is done - // between points). - - /** - * \brief Reduces a trajectory to N points or less. The resulting trajectory - * contains only point within the original trajectory (no interpolation is done - * between points). - * @param trajectory_in input trajectory - * @param trajectory_out filtered trajectory (N points or less - * @return true if successful - */ - bool update(const T& trajectory_in, T& trajectory_out); - - private: - /** - * @brief number of points to reduce trajectory to - */ - int n_points_; - - }; - -/** - * @brief Specializing trajectory filter implementation - */ -typedef NPointFilter NPointFilterAdapter; - -} - -#endif diff --git a/training/supplements/src/industrial_core/industrial_trajectory_filters/include/industrial_trajectory_filters/uniform_sample_filter.h b/training/supplements/src/industrial_core/industrial_trajectory_filters/include/industrial_trajectory_filters/uniform_sample_filter.h deleted file mode 100644 index 9469c066..00000000 --- a/training/supplements/src/industrial_core/industrial_trajectory_filters/include/industrial_trajectory_filters/uniform_sample_filter.h +++ /dev/null @@ -1,106 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2013, Southwest Research Institute - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in the - * documentation and/or other materials provided with the distribution. - * * Neither the name of the Southwest Research Institute, nor the names - * of its contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef UNIFORM_SAMPLE_FILTER_H_ -#define UNIFORM_SAMPLE_FILTER_H_ - -#include - -/* - * These headers were part of the trajectory filter interface from the - * arm navigation system. These can no longer be included in a catkin based - * package. - #include - #include - */ - -namespace industrial_trajectory_filters -{ - -/** - * \brief This is a simple filter which performs a uniforming sampling of - * a trajectory using linear interpolation. - * - */ -template - - class UniformSampleFilter : public industrial_trajectory_filters::FilterBase - { - public: - /** - * @brief Default constructor - */ - UniformSampleFilter(); - /** - * @brief Default destructor - */ - ~UniformSampleFilter(); - - virtual bool configure(); - - /** - * Uniformly samples(in terms of time) the input trajectory based upon the - * sample duration. Sampling is performed via interpolation ensuring smooth - * velocity and acceleration. NOTE: For this reason trajectories must be - * fully defined before using this filter. - * @param trajectory_in non uniform trajectory - * @param trajectory_out uniform(in terms of time) trajectory - * @return - */ - bool update(const T& trajectory_in, T& trajectory_out); - - /** - * @brief Perform interpolation between p1 and p2. Time from start must be - * in between p1 and p2 times. - * @param p1 prior trajectory point - * @param p2 subsequent trajectory point - * @param time_from_start time from start of trajectory (i.e. p0). - * @param interp_pt resulting interpolated point - * @return true if successful, otherwise false. - */ - bool interpolatePt(trajectory_msgs::JointTrajectoryPoint & p1, trajectory_msgs::JointTrajectoryPoint & p2, - double time_from_start, trajectory_msgs::JointTrajectoryPoint & interp_pt); - - private: - /** - * @brief uniform sample duration (sec) - */ - double sample_duration_; - }; - -/** - * @brief Specializing trajectory filter implementation - */ -typedef UniformSampleFilter UniformSampleFilterAdapter; - -} - -#endif diff --git a/training/supplements/src/industrial_core/industrial_trajectory_filters/mainpage.dox b/training/supplements/src/industrial_core/industrial_trajectory_filters/mainpage.dox deleted file mode 100644 index 893983c5..00000000 --- a/training/supplements/src/industrial_core/industrial_trajectory_filters/mainpage.dox +++ /dev/null @@ -1,14 +0,0 @@ -/** -\mainpage -\htmlinclude manifest.html - -\b industrial_trajectory_filters provides robot trajectory filters. It also includes plugin adapters for the moveit motion planning library. - - -\section codeapi Code API - -The main APIs of are largely captured in the following interface classes: -- industrial_trajectory_filters::NPointFilter : A simple filter that removes trajectory points until the trajectory is N or less points -- industrial_trajectory_filters::UniformSampleFilter : Resamples a trajectory uniformly (in time). -- industrial_trajectory_filters::FilterBase : A moveit adapter class for old arm navigation packages. -*/ diff --git a/training/supplements/src/industrial_core/industrial_trajectory_filters/package.xml b/training/supplements/src/industrial_core/industrial_trajectory_filters/package.xml deleted file mode 100644 index 67baf9bf..00000000 --- a/training/supplements/src/industrial_core/industrial_trajectory_filters/package.xml +++ /dev/null @@ -1,38 +0,0 @@ - - industrial_trajectory_filters - 0.3.3 - -

- ROS Industrial libraries/plugins for filtering trajectories. -

-

- This package is part of the ROS Industrial program and contains libraries - and moveit plugins for filtering robot trajectories. -

- - Shaun Edwards - Jorge Nicho - Shaun Edwards - BSD - http://ros.org/wiki/industrial_trajectory_filters - https://github.com/ros-industrial/industrial_core/issues - https://github.com/ros-industrial/industrial_core - - catkin - - trajectory_msgs - pluginlib - moveit_core - moveit_ros_planning - orocos_kdl - - trajectory_msgs - pluginlib - moveit_core - moveit_ros_planning - orocos_kdl - - - - - diff --git a/training/supplements/src/industrial_core/industrial_trajectory_filters/planning_request_adapters_plugin_description.xml b/training/supplements/src/industrial_core/industrial_trajectory_filters/planning_request_adapters_plugin_description.xml deleted file mode 100644 index 5a26761c..00000000 --- a/training/supplements/src/industrial_core/industrial_trajectory_filters/planning_request_adapters_plugin_description.xml +++ /dev/null @@ -1,24 +0,0 @@ - - - - - This is a simple filter which reduces a trajectory to N points or less. - ROS parameters: - - n_points (default = 2) - - - - - - This is a simple filter which performs a uniforming sampling of - a trajectory using linear interpolation. - ROS parameters: - - sample_duration (default = 0.050) - - - - diff --git a/training/supplements/src/industrial_core/industrial_trajectory_filters/src/n_point_filter.cpp b/training/supplements/src/industrial_core/industrial_trajectory_filters/src/n_point_filter.cpp deleted file mode 100644 index 41d86dbe..00000000 --- a/training/supplements/src/industrial_core/industrial_trajectory_filters/src/n_point_filter.cpp +++ /dev/null @@ -1,136 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2011, Southwest Research Institute - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in the - * documentation and/or other materials provided with the distribution. - * * Neither the name of the Southwest Research Institute, nor the names - * of its contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -//#include -#include -#include - -using namespace industrial_trajectory_filters; - -const int DEFAULT_N = 2; - -template - NPointFilter::NPointFilter() : - FilterBase() - { - ROS_INFO_STREAM("Constructing N point filter"); - n_points_ = DEFAULT_N; - this->filter_name_ = "NPointFilter"; - this->filter_type_ = "NPointFilter"; - } - -template - NPointFilter::~NPointFilter() - { - } - -template - bool NPointFilter::configure() - { - //if (!filters::FilterBase::getParam("n_points", n_points_)) - if (!this->nh_.getParam("n_points", n_points_)) - { - ROS_WARN_STREAM("NPointFilter, params has no attribute n_points."); - } - if (n_points_ < 2) - { - ROS_WARN_STREAM( "n_points attribute less than min(2), setting to minimum"); - n_points_ = 2; - } - ROS_INFO_STREAM("Using a n_points value of " << n_points_); - - return true; - } - -template - bool NPointFilter::update(const T& trajectory_in, T& trajectory_out) - { - bool success = false; - int size_in = trajectory_in.request.trajectory.points.size(); - - // Copy non point related data - trajectory_out.request.trajectory = trajectory_in.request.trajectory; - // Clear out the trajectory points - trajectory_out.request.trajectory.points.clear(); - - if (size_in > n_points_) - { - //Add first point to output trajectory - trajectory_out.request.trajectory.points.push_back(trajectory_in.request.trajectory.points.front()); - - int intermediate_points = n_points_ - 2; //subtract the first and last elements - double int_point_increment = double(size_in) / double(intermediate_points + 1.0); - ROS_INFO_STREAM( - "Number of intermediate points: " << intermediate_points << ", increment: " << int_point_increment); - - // The intermediate point index is determined by the following equation: - // int_point_index = i * int_point_increment - // Ex: n_points_ = 4, size_in = 11, intermediate_points = 2 -> - // int_point_increment = 3.66667, - // i = 1: int_point_index = 3 - // i = 2: int_point_index = 7 - for (int i = 1; i <= intermediate_points; i++) - { - int int_point_index = int(double(i) * int_point_increment); - ROS_INFO_STREAM("Intermediate point index: " << int_point_index); - trajectory_out.request.trajectory.points.push_back(trajectory_in.request.trajectory.points[int_point_index]); - } - - //Add last point to output trajectory - trajectory_out.request.trajectory.points.push_back(trajectory_in.request.trajectory.points.back()); - - ROS_INFO_STREAM( - "Filtered trajectory from: " << trajectory_in.request.trajectory.points.size() << " to: " << trajectory_out.request.trajectory.points.size()); - - success = true; - } - else - { - ROS_WARN_STREAM( "Trajectory size less than n: " << n_points_ << ", pass through"); - trajectory_out.request.trajectory = trajectory_in.request.trajectory; - success = true; - } - - return success; - } - -// registering planner adapter -CLASS_LOADER_REGISTER_CLASS(industrial_trajectory_filters::NPointFilterAdapter, - planning_request_adapter::PlanningRequestAdapter); - -/* - * Old plugin declaration for arm navigation trajectory filters - PLUGINLIB_DECLARE_CLASS(industrial_trajectory_filters, - IndustrialNPointFilterJointTrajectoryWithConstraints, - industrial_trajectory_filters::NPointFilter, - filters::FilterBase); - - */ diff --git a/training/supplements/src/industrial_core/industrial_trajectory_filters/src/uniform_sample_filter.cpp b/training/supplements/src/industrial_core/industrial_trajectory_filters/src/uniform_sample_filter.cpp deleted file mode 100644 index 157c32cc..00000000 --- a/training/supplements/src/industrial_core/industrial_trajectory_filters/src/uniform_sample_filter.cpp +++ /dev/null @@ -1,227 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2013, Southwest Research Institute - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in the - * documentation and/or other materials provided with the distribution. - * * Neither the name of the Southwest Research Institute, nor the names - * of its contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#include -#include -#include - -using namespace industrial_trajectory_filters; - -const double DEFAULT_SAMPLE_DURATION = 0.050; //seconds - -template - UniformSampleFilter::UniformSampleFilter() : - industrial_trajectory_filters::FilterBase() - { - ROS_INFO_STREAM("Constructing N point filter"); - sample_duration_ = DEFAULT_SAMPLE_DURATION; - this->filter_name_ = "UniformSampleFilter"; - this->filter_type_ = "UniformSampleFilter"; - } - -template - UniformSampleFilter::~UniformSampleFilter() - { - } - -template - bool UniformSampleFilter::configure() - { - if (!this->nh_.getParam("sample_duration", sample_duration_)) - { - ROS_WARN_STREAM( "UniformSampleFilter, params has no attribute sample_duration."); - } - ROS_INFO_STREAM("Using a sample_duration value of " << sample_duration_); - - return true; - } - -template - bool UniformSampleFilter::update(const T& trajectory_in, T& trajectory_out) - { - bool success = false; - size_t size_in = trajectory_in.request.trajectory.points.size(); - double duration_in = trajectory_in.request.trajectory.points.back().time_from_start.toSec(); - double interpolated_time = 0.0; - size_t index_in = 0; - - trajectory_msgs::JointTrajectoryPoint p1, p2, interp_pt; - - trajectory_out = trajectory_in; - - // Clear out the trajectory points - trajectory_out.request.trajectory.points.clear(); - - while (interpolated_time < duration_in) - { - ROS_INFO_STREAM("Interpolated time: " << interpolated_time); - // Increment index until the interpolated time is past the start time. - while (interpolated_time > trajectory_in.request.trajectory.points[index_in + 1].time_from_start.toSec()) - { - ROS_INFO_STREAM( - "Interpolated time: " << interpolated_time << ", next point time: " << (trajectory_in.request.trajectory.points[index_in + 1].time_from_start.toSec())); - ROS_INFO_STREAM("Incrementing index"); - index_in++; - if (index_in >= size_in) - { - ROS_ERROR_STREAM( - "Programming error, index: " << index_in << ", greater(or equal) to size: " << size_in << " input duration: " << duration_in << " interpolated time:)" << interpolated_time); - return false; - } - } - p1 = trajectory_in.request.trajectory.points[index_in]; - p2 = trajectory_in.request.trajectory.points[index_in + 1]; - if (!interpolatePt(p1, p2, interpolated_time, interp_pt)) - { - ROS_ERROR_STREAM("Failed to interpolate point"); - return false; - } - trajectory_out.request.trajectory.points.push_back(interp_pt); - interpolated_time += sample_duration_; - - } - - ROS_INFO_STREAM( - "Interpolated time exceeds original trajectory (quitting), original: " << duration_in << " final interpolated time: " << interpolated_time); - p2 = trajectory_in.request.trajectory.points.back(); - p2.time_from_start = ros::Duration(interpolated_time); - // TODO: Really should check that appending the last point doesn't result in - // really slow motion at the end. This could happen if the sample duration is a - // large percentage of the trajectory duration (not likely). - trajectory_out.request.trajectory.points.push_back(p2); - - ROS_INFO_STREAM( - "Uniform sampling, resample duraction: " << sample_duration_ << " input traj. size: " << trajectory_in.request.trajectory.points.size() << " output traj. size: " << trajectory_out.request.trajectory.points.size()); - - success = true; - return success; - } - -template - bool UniformSampleFilter::interpolatePt(trajectory_msgs::JointTrajectoryPoint & p1, - trajectory_msgs::JointTrajectoryPoint & p2, double time_from_start, - trajectory_msgs::JointTrajectoryPoint & interp_pt) - { - bool rtn = false; - double p1_time_from_start = p1.time_from_start.toSec(); - double p2_time_from_start = p2.time_from_start.toSec(); - - ROS_INFO_STREAM("time from start: " << time_from_start); - - if (time_from_start >= p1_time_from_start && time_from_start <= p2_time_from_start) - { - if (p1.positions.size() == p1.velocities.size() && p1.positions.size() == p1.accelerations.size()) - { - if (p1.positions.size() == p2.positions.size() && p1.velocities.size() == p2.velocities.size() - && p1.accelerations.size() == p2.accelerations.size()) - { - // Copy p1 to ensure the interp_pt has the correct size vectors - interp_pt = p1; - // TODO: Creating a new spline calculator in this function means that - // it may be created multiple times for the same points (assuming the - // resample duration is less that the actual duration, which it might - // be sometimes) - KDL::VelocityProfile_Spline spline_calc; - ROS_INFO_STREAM( "---------------Begin interpolating joint point---------------"); - - for (size_t i = 0; i < p1.positions.size(); ++i) - { - // Calculated relative times for spline calculation - double time_from_p1 = time_from_start - p1.time_from_start.toSec(); - double time_from_p1_to_p2 = p2_time_from_start - p1_time_from_start; - - ROS_INFO_STREAM("time from p1: " << time_from_p1); - ROS_INFO_STREAM( "time_from_p1_to_p2: " << time_from_p1_to_p2); - - spline_calc.SetProfileDuration(p1.positions[i], p1.velocities[i], p1.accelerations[i], p2.positions[i], - p2.velocities[i], p2.accelerations[i], time_from_p1_to_p2); - - ros::Duration time_from_start_dur(time_from_start); - ROS_INFO_STREAM( "time from start_dur: " << time_from_start_dur); - - interp_pt.time_from_start = time_from_start_dur; - interp_pt.positions[i] = spline_calc.Pos(time_from_p1); - interp_pt.velocities[i] = spline_calc.Vel(time_from_p1); - interp_pt.accelerations[i] = spline_calc.Acc(time_from_p1); - - ROS_INFO_STREAM( - "p1.pos: " << p1.positions[i] << ", vel: " << p1.velocities[i] << ", acc: " << p1.accelerations[i] << ", tfs: " << p1.time_from_start); - - ROS_INFO_STREAM( - "p2.pos: " << p2.positions[i] << ", vel: " << p2.velocities[i] << ", acc: " << p2.accelerations[i] << ", tfs: " << p2.time_from_start); - - ROS_INFO_STREAM( - "interp_pt.pos: " << interp_pt.positions[i] << ", vel: " << interp_pt.velocities[i] << ", acc: " << interp_pt.accelerations[i] << ", tfs: " << interp_pt.time_from_start); - } - ROS_INFO_STREAM( "---------------End interpolating joint point---------------"); - rtn = true; - } - else - { - ROS_ERROR_STREAM("Trajectory point size mismatch"); - ROS_ERROR_STREAM( - "Trajectory point 1, pos: " << p1.positions.size() << " vel: " << p1.velocities.size() << " acc: " << p1.accelerations.size()); - ROS_ERROR_STREAM( - "Trajectory point 2, pos: " << p2.positions.size() << " vel: " << p2.velocities.size() << " acc: " << p2.accelerations.size()); - rtn = false; - } - - } - else - { - ROS_ERROR_STREAM( - "Trajectory point not fully defined, pos: " << p1.positions.size() << " vel: " << p1.velocities.size() << " acc: " << p1.accelerations.size()); - rtn = false; - } - } - else - { - ROS_ERROR_STREAM( - "Time: " << time_from_start << " not between interpolation point times[" << p1.time_from_start.toSec() << "," << p2.time_from_start.toSec() << "]"); - rtn = false; - } - - return rtn; - } - -// registering planner adapter -CLASS_LOADER_REGISTER_CLASS( industrial_trajectory_filters::UniformSampleFilterAdapter, - planning_request_adapter::PlanningRequestAdapter); - -/* - * Old plugin declaration for arm navigation trajectory filters - PLUGINLIB_DECLARE_CLASS(industrial_trajectory_filters, - IndustrialNPointFilterJointTrajectoryWithConstraints, - industrial_trajectory_filters::NPointFilter, - filters::FilterBase); - - */ - diff --git a/training/supplements/src/industrial_core/industrial_utils/CHANGELOG.rst b/training/supplements/src/industrial_core/industrial_utils/CHANGELOG.rst deleted file mode 100644 index 5ce48b6c..00000000 --- a/training/supplements/src/industrial_core/industrial_utils/CHANGELOG.rst +++ /dev/null @@ -1,19 +0,0 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package industrial_utils -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -0.3.3 (2014-01-13) ------------------- -* No change - -0.3.2 (2014-01-10) ------------------- -* Removed header from industrial_utils/utils.h (not required) -* Contributors: Shaun Edwards - -0.3.1 (2014-01-09) ------------------- -* Remove obsolete export tags. Fix `#43 `_. -* Fix `#52 `_: ignore fixed joints in param_utils::getJointVelocityLimits -* Converted to catkin -* Contributors: JeremyZoss, Shaun Edwards, gavanderhoorn diff --git a/training/supplements/src/industrial_core/industrial_utils/CMakeLists.txt b/training/supplements/src/industrial_core/industrial_utils/CMakeLists.txt deleted file mode 100644 index cac163fe..00000000 --- a/training/supplements/src/industrial_core/industrial_utils/CMakeLists.txt +++ /dev/null @@ -1,34 +0,0 @@ -cmake_minimum_required(VERSION 2.8.3) - -project(industrial_utils) - -find_package(catkin REQUIRED COMPONENTS roscpp urdf) - -find_package(Boost REQUIRED COMPONENTS system) - -catkin_package( - CATKIN_DEPENDS roscpp urdf - INCLUDE_DIRS include - LIBRARIES ${PROJECT_NAME} -) - -include_directories(include - ${catkin_INCLUDE_DIRS}) - -add_library(${PROJECT_NAME} src/utils.cpp src/param_utils.cpp) -target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES}) - - - -catkin_add_gtest(utest_inds_utils test/utest.cpp) -target_link_libraries(utest_inds_utils ${PROJECT_NAME} ${catkin_LIBRARIES}) - - - -install( - TARGETS ${PROJECT_NAME} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) - -install( - DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}) diff --git a/training/supplements/src/industrial_core/industrial_utils/include/industrial_utils/param_utils.h b/training/supplements/src/industrial_core/industrial_utils/include/industrial_utils/param_utils.h deleted file mode 100644 index d5c0dc7e..00000000 --- a/training/supplements/src/industrial_core/industrial_utils/include/industrial_utils/param_utils.h +++ /dev/null @@ -1,82 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2012, Southwest Research Institute - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in the - * documentation and/or other materials provided with the distribution. - * * Neither the name of the Southwest Research Institute, nor the names - * of its contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - - -#ifndef PARAM_UTILS_H_ -#define PARAM_UTILS_H_ - -#include -#include -#include - -namespace industrial_utils -{ -namespace param -{ - -/** - * \brief Gets parameter list as vector of strings - * - * \param param_name name of list parameter - * \param list_param populated with parameter value(s) - * - * \return true if parameter - */ -bool getListParam(const std::string param_name, std::vector & list_param); - -/** - * \brief Tries to get a set of joint names using several fallback methods: - * 1) check parameter for an explicit list - * 2) try to parse from given URDF data - * 3) use default joint names: ["joint_1", "joint_2", ..., "joint_6"] - * - * \param[in] joint_list_param name of joint-names-list parameter to check - * \param[in] urdf_param name of URDF description parameter to check - * \param[out] joint_names list of joint names - * - * \return true if parameter found, false if defaults used - */ -bool getJointNames(const std::string joint_list_param, const std::string urdf_param, - std::vector & joint_names); - -/** - * \brief Tries to read joint velocity limits from the specified URDF parameter - * - * \param[in] urdf_param_name name of URDF parameter - * \param[out] velocity_limits map of velocity limits for each URDF joint - * - * \return true if parameter found, false if not found - */ -bool getJointVelocityLimits(const std::string urdf_param_name, std::map &velocity_limits); - -} //industrial_utils::param -} //industrial_utils -#endif /* PARAM_UTILS_H_ */ diff --git a/training/supplements/src/industrial_core/industrial_utils/include/industrial_utils/utils.h b/training/supplements/src/industrial_core/industrial_utils/include/industrial_utils/utils.h deleted file mode 100644 index 8c9f3af5..00000000 --- a/training/supplements/src/industrial_core/industrial_utils/include/industrial_utils/utils.h +++ /dev/null @@ -1,83 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2012, Southwest Research Institute - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in the - * documentation and/or other materials provided with the distribution. - * * Neither the name of the Southwest Research Institute, nor the names - * of its contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef INDUSTRIAL_UTILS_H_ -#define INDUSTRIAL_UTILS_H_ - -#include -#include -#include "urdf/model.h" - -namespace industrial_utils -{ - -/** - * \brief Checks to see if sets are similar(same members, any order) - * NOTE: Vectors are passed by copy because they are modified by the - * function(i.e. ordered). - * This function should not be used for large vectors or in loops (it - * is slow!) - * - * \param lhs first vector - * \param rhs second vector - * - * \return true set are similar (same members, any order) - */ -bool isSimilar(std::vector lhs, std::vector rhs); - -/** - * \brief Checks to see if sets are the same(same members, same order) - * Wraps std::equal method. - * - * \param lhs first vector - * \param rhs second vector - * - * \return true set are similar (same members, same order) - */ -bool isSame(const std::vector & lhs, const std::vector & rhs); - -/* - * \brief Returns joint names for a simple serial chain from a URDF tree - * - returns an error if branching tree is found - * - assumes chain runs root->leaf. leaf->root->leaf chains not allowed. - * - * \param[in] link URDF model link to search from (e.g. model.getRoot()) - * \param[in] ignore_fixed flag to ignore fixed joints - * \param[in,out] joint_names vector of joint names - * - * \return true if successful, false if error occurred (e.g. branching tree) - */ -bool findChainJointNames(const boost::shared_ptr &link, bool ignore_fixed, - std::vector &joint_names); - -} //industrial_utils - -#endif /* INDUSTRIAL_UTILS_H_ */ diff --git a/training/supplements/src/industrial_core/industrial_utils/mainpage.dox b/training/supplements/src/industrial_core/industrial_utils/mainpage.dox deleted file mode 100644 index 01a59cf7..00000000 --- a/training/supplements/src/industrial_core/industrial_utils/mainpage.dox +++ /dev/null @@ -1,18 +0,0 @@ -/** -\mainpage -\htmlinclude manifest.html - -\b industrial_utils contains utility functions and classes for the ROS-Industrial program - - - - -\section codeapi Code API - -The main APIs of are largely captured in the following interface classes: -- industrial_utils : General utilities -- industrial_utils::param : Utilities for parsing ROS parameters - -*/ diff --git a/training/supplements/src/industrial_core/industrial_utils/package.xml b/training/supplements/src/industrial_core/industrial_utils/package.xml deleted file mode 100644 index 48006363..00000000 --- a/training/supplements/src/industrial_core/industrial_utils/package.xml +++ /dev/null @@ -1,16 +0,0 @@ - - industrial_utils - 0.3.3 - Industrial utils is a library package that captures common funcitonality for the ROS-Industrial distribution. - - Shaun Edwards - BSD - http://ros.org/wiki/industrial_utils - Shaun M. Edwards - - catkin - roscpp - urdf - roscpp - urdf - diff --git a/training/supplements/src/industrial_core/industrial_utils/src/param_utils.cpp b/training/supplements/src/industrial_core/industrial_utils/src/param_utils.cpp deleted file mode 100644 index 17e63702..00000000 --- a/training/supplements/src/industrial_core/industrial_utils/src/param_utils.cpp +++ /dev/null @@ -1,156 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2012, Southwest Research Institute - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in the - * documentation and/or other materials provided with the distribution. - * * Neither the name of the Southwest Research Institute, nor the names - * of its contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#include - -#include "industrial_utils/param_utils.h" -#include "industrial_utils/utils.h" -#include "ros/ros.h" -#include "urdf/model.h" - -namespace industrial_utils -{ -namespace param -{ -bool getListParam(const std::string param_name, std::vector & list_param) -{ - bool rtn = false; - XmlRpc::XmlRpcValue rpc_list; - - list_param.clear(); //clear out return value - - rtn = ros::param::get(param_name, rpc_list); - - if (rtn) - { - rtn = (rpc_list.getType() == XmlRpc::XmlRpcValue::TypeArray); - - if (rtn) - { - - for (int i = 0; i < rpc_list.size(); ++i) - { - rtn = (rpc_list[i].getType() == XmlRpc::XmlRpcValue::TypeString); - if (rtn) - { - ROS_INFO_STREAM("Adding " << rpc_list[i] << " to list parameter"); - list_param.push_back(static_cast(rpc_list[i])); - } - else - { - ROS_ERROR_STREAM("List item for: " << param_name << " not of string type"); - } - } - } - else - { - ROS_ERROR_STREAM("Parameter: " << param_name << " not of list type"); - } - } - else - { - ROS_ERROR_STREAM("Failed to get parameter: " << param_name); - } - - return rtn; - -} - -std::string vec2str(const std::vector &vec) -{ - std::string s, delim = ", "; - std::stringstream ss; - std::copy(vec.begin(), vec.end(), std::ostream_iterator(ss, delim.c_str())); - s = ss.str(); - return "[" + s.erase(s.length()-2) + "]"; -} - -bool getJointNames(const std::string joint_list_param, const std::string urdf_param, - std::vector & joint_names) -{ - joint_names.clear(); - - // 1) Try to read explicit list of joint names - if (ros::param::has(joint_list_param) && getListParam(joint_list_param, joint_names)) - { - ROS_INFO_STREAM("Found user-specified joint names in '" << joint_list_param << "': " << vec2str(joint_names)); - return true; - } - else - ROS_WARN_STREAM("Unable to find user-specified joint names in '" << joint_list_param << "'"); - - // 2) Try to find joint names from URDF model - urdf::Model model; - if ( ros::param::has(urdf_param) - && model.initParam(urdf_param) - && findChainJointNames(model.getRoot(), true, joint_names) ) - { - ROS_INFO_STREAM("Using joint names from URDF: '" << urdf_param << "': " << vec2str(joint_names)); - return true; - } - else - ROS_WARN_STREAM("Unable to find URDF joint names in '" << urdf_param << "'"); - - // 3) Use default joint-names - const int NUM_JOINTS = 6; //Most robots have 6 joints - for (int i=0; i &velocity_limits) -{ - urdf::Model model; - std::map >::iterator iter; - - if (!ros::param::has(urdf_param_name) || !model.initParam(urdf_param_name)) - return false; - - velocity_limits.clear(); - for (iter=model.joints_.begin(); iter!=model.joints_.end(); ++iter) - { - std::string joint_name(iter->first); - boost::shared_ptr limits = iter->second->limits; - if ( limits && (limits->velocity > 0) ) - velocity_limits.insert(std::pair(joint_name,limits->velocity)); - } - - return true; -} - -} //industrial_utils::param -} //industrial_utils diff --git a/training/supplements/src/industrial_core/industrial_utils/src/utils.cpp b/training/supplements/src/industrial_core/industrial_utils/src/utils.cpp deleted file mode 100644 index b2b0335f..00000000 --- a/training/supplements/src/industrial_core/industrial_utils/src/utils.cpp +++ /dev/null @@ -1,128 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2012, Southwest Research Institute - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in the - * documentation and/or other materials provided with the distribution. - * * Neither the name of the Southwest Research Institute, nor the names - * of its contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#include "industrial_utils/utils.h" -#include "ros/ros.h" -#include - -namespace industrial_utils -{ - -bool isSimilar(std::vector lhs, std::vector rhs) -{ - bool rtn = false; - - if (lhs.size() != rhs.size()) - { - rtn = false; - } - else - { - std::sort(lhs.begin(), lhs.end()); - std::sort(rhs.begin(), rhs.end()); - rtn = isSame(lhs, rhs); - } - - return rtn; -} - -bool isSame(const std::vector & lhs, const std::vector & rhs) -{ - bool rtn = false; - - if (lhs.size() != rhs.size()) - { - rtn = false; - } - else - { - rtn = std::equal(lhs.begin(), lhs.end(), rhs.begin()); - } - return rtn; -} - -bool findChainJointNames(const boost::shared_ptr &link, bool ignore_fixed, - std::vector &joint_names) -{ - typedef std::vector > joint_list; - typedef std::vector > link_list; - std::string found_joint, found_link; - - // check for joints directly connected to this link - const joint_list &joints = link->child_joints; - ROS_DEBUG("Found %d child joints:", joints.size()); - for (joint_list::const_iterator it=joints.begin(); it!=joints.end(); ++it) - { - ROS_DEBUG_STREAM(" " << (*it)->name << ": type " << (*it)->type); - if (ignore_fixed && (*it)->type == urdf::Joint::FIXED) - continue; - - if (found_joint.empty()) - { - found_joint = (*it)->name; - joint_names.push_back(found_joint); - } - else - { - ROS_WARN_STREAM("Unable to find chain in URDF. Branching joints: " << found_joint << " and " << (*it)->name); - return false; // branching tree (multiple valid child-joints) - } - } - - // check for joints connected to children of this link - const link_list &links = link->child_links; - std::vector sub_joints; - ROS_DEBUG("Found %d child links:", links.size()); - for (link_list::const_iterator it=links.begin(); it!=links.end(); ++it) - { - ROS_DEBUG_STREAM(" " << (*it)->name); - if (!findChainJointNames(*it, ignore_fixed, sub_joints)) // NOTE: recursive call - return false; - - if (sub_joints.empty()) - continue; - - if (found_link.empty()) - { - found_link = (*it)->name; - joint_names.insert(joint_names.end(), sub_joints.begin(), sub_joints.end()); // append sub_joints - } - else - { - ROS_WARN_STREAM("Unable to find chain in URDF. Branching links: " << found_link << " and " << (*it)->name); - return false; // branching tree (multiple valid child-joints) - } - } - - return true; -} - -} //industrial_utils diff --git a/training/supplements/src/industrial_core/industrial_utils/test/test_joint_names.cpp b/training/supplements/src/industrial_core/industrial_utils/test/test_joint_names.cpp deleted file mode 100644 index 838c6669..00000000 --- a/training/supplements/src/industrial_core/industrial_utils/test/test_joint_names.cpp +++ /dev/null @@ -1,60 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2013, Southwest Research Institute - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in the - * documentation and/or other materials provided with the distribution. - * * Neither the name of the Southwest Research Institute, nor the names - * of its contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#include "industrial_utils/utils.h" -#include "ros/ros.h" - -using namespace industrial_utils; - -#define ROS_ERROR_EXIT(...) do {ROS_ERROR(__VA_ARGS__); exit(-1); } while (0) - -// Quick program to test joint-name extraction from URDF -int main(int argc, char **argv) -{ - if (argc != 2) - ROS_ERROR_EXIT("Usage: test_joint_names my_robot.urdf"); - - urdf::Model model; - if (!model.initFile(argv[1])) - ROS_ERROR_EXIT("Unable to open URDF file: %s", argv[1]); - - std::vector joint_names; - if (!findChainJointNames(model.getRoot(), true, joint_names)) - ROS_ERROR_EXIT("Unable to read Joint Names from URDF"); - - printf("Joint names: "); - std::ostringstream s; - std::copy(joint_names.begin(), joint_names.end(), std::ostream_iterator(s, ", ")); - std::string out = s.str(); - std::cout << "[" << out.erase(out.size()-2) << "]" << std::endl; - - return 1; -} diff --git a/training/supplements/src/industrial_core/industrial_utils/test/utest.cpp b/training/supplements/src/industrial_core/industrial_utils/test/utest.cpp deleted file mode 100644 index 325305d2..00000000 --- a/training/supplements/src/industrial_core/industrial_utils/test/utest.cpp +++ /dev/null @@ -1,89 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2012, Southwest Research Institute - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in the - * documentation and/or other materials provided with the distribution. - * * Neither the name of the Southwest Research Institute, nor the names - * of its contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#include "industrial_utils/utils.h" - -#include - -using namespace industrial_utils; - -TEST(IndustrialUtilsSuite, vector_compare) -{ - - std::vector v1; - std::vector v2; - std::vector v3; - - // Testing empty vectors - EXPECT_TRUE(isSimilar(v1, v2)); - EXPECT_TRUE(isSame(v1, v2)); - - std::string i1 = "item_1"; - std::string i2 = "item_2"; - std::string i3 = "item_3"; - - v1.push_back(i1); - v1.push_back(i2); - v1.push_back(i3); - - // Testing one empty, one populated - EXPECT_FALSE(isSimilar(v1, v2)); - EXPECT_FALSE(isSame(v1, v2)); - - v2.push_back(i1); - v2.push_back(i2); - - // Testing one partially filled, one full - EXPECT_FALSE(isSimilar(v1, v2)); - EXPECT_FALSE(isSame(v1, v2)); - - v2.push_back(i3); - - // Testing same vectors - EXPECT_TRUE(isSimilar(v1, v2)); - EXPECT_TRUE(isSame(v1, v2)); - - v3.push_back(i3); - v3.push_back(i2); - v3.push_back(i1); - - // Testing similar but not same - EXPECT_TRUE(isSimilar(v1, v3)); - EXPECT_FALSE(isSame(v1, v3)); - -} - -// Run all the tests that were declared with TEST() -int main(int argc, char **argv) -{ - testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); -} diff --git a/training/supplements/src/industrial_core/simple_message/CHANGELOG.rst b/training/supplements/src/industrial_core/simple_message/CHANGELOG.rst deleted file mode 100644 index 08a17212..00000000 --- a/training/supplements/src/industrial_core/simple_message/CHANGELOG.rst +++ /dev/null @@ -1,20 +0,0 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package simple_message -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -0.3.3 (2014-01-13) ------------------- -* Fixed build issue due simple message library linking -* Contributors: gavanderhoorn - -0.3.2 (2014-01-10) ------------------- -* Removed header from industrial_utils/utils.h (not required) - -0.3.1 (2014-01-09) ------------------- -* Added polling check to socket read and muiltiple read calls in order to receive all desired bytes -* Removed library export from catkin macro. Packages that depend on these must declare library dependencies explicitly (by name) -* Add error message to socket errors (instead of just errno). -* Converted to catkin -* Contributors: Christina Gomez, JeremyZoss, ROS, Shaun Edwards, gavanderhoorn, jrgnicho, kphawkins, shaun-edwards diff --git a/training/supplements/src/industrial_core/simple_message/CMakeLists.txt b/training/supplements/src/industrial_core/simple_message/CMakeLists.txt deleted file mode 100644 index e184397b..00000000 --- a/training/supplements/src/industrial_core/simple_message/CMakeLists.txt +++ /dev/null @@ -1,137 +0,0 @@ -cmake_minimum_required(VERSION 2.8.3) - -project(simple_message) - -find_package(catkin REQUIRED COMPONENTS roscpp industrial_msgs) - -# Build static libs, to reduce dependency-chaining for industrial_robot_client -set(ROS_BUILD_STATIC_LIBS true) -set(ROS_BUILD_SHARED_LIBS false) - -# The simple_message library is designed to cross compile on Ubuntu -# and various robot controllers. This requires conditionally compiling -# certain functions and headers. The definition below enables compiling -# for a ROS node. -add_definitions(-DROS=1) #build using ROS libraries -add_definitions(-DLINUXSOCKETS=1) #use linux sockets for communication - -set(SRC_FILES src/byte_array.cpp - src/simple_message.cpp - src/smpl_msg_connection.cpp - - src/socket/simple_socket.cpp - src/socket/udp_socket.cpp - src/socket/udp_client.cpp - src/socket/udp_server.cpp - src/socket/tcp_socket.cpp - src/socket/tcp_client.cpp - src/socket/tcp_server.cpp - - src/message_handler.cpp - src/message_manager.cpp - src/ping_handler.cpp - src/ping_message.cpp - src/joint_data.cpp - src/joint_feedback.cpp - src/joint_traj_pt.cpp - src/joint_traj_pt_full.cpp - src/joint_traj.cpp - src/robot_status.cpp - - src/messages/joint_message.cpp - src/messages/joint_feedback_message.cpp - src/messages/joint_traj_pt_message.cpp - src/messages/joint_traj_pt_full_message.cpp - src/messages/robot_status_message.cpp - - src/simple_comms_fault_handler.cpp) - -set(UTEST_SRC_FILES test/utest.cpp test/utest_message.cpp) - -# The simple message make file builds two libraries: simple_message and -# simple_message_byte_swapping. -# -# simple_message - is the default library. This library should be used -# when the target for the simple message is the same endian (i.e. both -# big-endian or little-endian). Intel based machines are little endian -# -# simple_message_byte_swapping - is an alternative library that can be used -# when the target for simple message is a DIFFERENT endian AND when the target -# target cannot perform byte swapping (as is the case for some industrial -# controllers). This library performs byte swapping at the lowest load/unload -# levels. - -# NOTE: The libraries generated this package are not included in the catkin_package -# macro because libraries must be explicitly linked in projects that depend on this -# package. If this is not done (and these libraries were exported), then multiple -# library definitions (normal - simple_message and byteswapped - simple_message_bswap) -# are both included (this is bad). - -catkin_package( - CATKIN_DEPENDS roscpp industrial_msgs - INCLUDE_DIRS include - LIBRARIES ${PROJECT_NAME}_dummy - CFG_EXTRAS issue46_workaround.cmake -) - - -include_directories(include - ${catkin_INCLUDE_DIRS} -) - - -# generate dummy library (we export it in catkin_package(..)), to force catkin -# to set up LIBRARY_DIRS properly. -# TODO: find out if LIBRARY_DIRS can be exported without dummy library target -add_custom_command( - OUTPUT ${CMAKE_CURRENT_BINARY_DIR}/${PROJECT_NAME}_dummy.cpp - COMMAND ${CMAKE_COMMAND} -E touch ${CMAKE_CURRENT_BINARY_DIR}/${PROJECT_NAME}_dummy.cpp) -add_library(${PROJECT_NAME}_dummy ${CMAKE_CURRENT_BINARY_DIR}/${PROJECT_NAME}_dummy.cpp) -# unfortunately this will have to be installed, but the linker will remove it -# from the library dependencies of dependent targets, as it contains no symbols -# that can be imported from it. -install(TARGETS ${PROJECT_NAME}_dummy DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) - - -# NOTE: All test files require TEST_PORT_BASE to be defined. Defining different -# ports for each test executable allows them to run in parallel. - -# DEFAULT LIBRARY (SAME ENDIAN) -add_library(simple_message ${SRC_FILES}) -target_link_libraries(simple_message ${catkin_LIBRARIES}) -add_dependencies(simple_message ${industrial_msgs_EXPORTED_TARGETS}) - -catkin_add_gtest(utest ${UTEST_SRC_FILES}) -set_target_properties(utest PROPERTIES COMPILE_DEFINITIONS "TEST_PORT_BASE=11000") -target_link_libraries(utest simple_message) - -# ALTERNATIVE LIBRARY (DIFFERENT ENDIAN) -add_library(simple_message_bswap ${SRC_FILES}) -set_target_properties(simple_message_bswap PROPERTIES COMPILE_DEFINITIONS "BYTE_SWAPPING") -target_link_libraries(simple_message_bswap ${catkin_LIBRARIES}) -add_dependencies(simple_message_bswap ${industrial_msgs_EXPORTED_TARGETS}) - -catkin_add_gtest(utest_byte_swapping ${UTEST_SRC_FILES}) -set_target_properties(utest_byte_swapping PROPERTIES COMPILE_DEFINITIONS "TEST_PORT_BASE=12000") -target_link_libraries(utest_byte_swapping simple_message_bswap) - -# ALTERNATIVE LIBRARY (64-bit floats) -add_library(simple_message_float64 ${SRC_FILES}) -set_target_properties(simple_message_float64 PROPERTIES COMPILE_DEFINITIONS "FLOAT64") -target_link_libraries(simple_message_float64 ${catkin_LIBRARIES}) -add_dependencies(simple_message_float64 ${industrial_msgs_EXPORTED_TARGETS}) - -catkin_add_gtest(utest_float64 ${UTEST_SRC_FILES}) -set_target_properties(utest_float64 PROPERTIES COMPILE_DEFINITIONS "TEST_PORT_BASE=13000;FLOAT64") -target_link_libraries(utest_float64 simple_message_float64) - - - -install( - TARGETS simple_message simple_message_bswap simple_message_float64 - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) - -install( - DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}) - diff --git a/training/supplements/src/industrial_core/simple_message/cmake/issue46_workaround.cmake b/training/supplements/src/industrial_core/simple_message/cmake/issue46_workaround.cmake deleted file mode 100644 index 7ddf14be..00000000 --- a/training/supplements/src/industrial_core/simple_message/cmake/issue46_workaround.cmake +++ /dev/null @@ -1,10 +0,0 @@ -# -# Temporary workaround for issue ros-industrial/industrial_core#46. -# -message(STATUS "simple_message: work around for #46") -if (DEFINED simple_message_LIBRARY_DIRS) -else() - message(FATAL_ERROR "simple_message_LIBRARY_DIRS not set, " - "have you find_package()-ed it?") -endif() -link_directories(${simple_message_LIBRARY_DIRS}) diff --git a/training/supplements/src/industrial_core/simple_message/include/simple_message/byte_array.h b/training/supplements/src/industrial_core/simple_message/include/simple_message/byte_array.h deleted file mode 100755 index fd53cd7c..00000000 --- a/training/supplements/src/industrial_core/simple_message/include/simple_message/byte_array.h +++ /dev/null @@ -1,414 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2011, Southwest Research Institute - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in the - * documentation and/or other materials provided with the distribution. - * * Neither the name of the Southwest Research Institute, nor the names - * of its contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef BYTE_ARRAY_H -#define BYTE_ARRAY_H - -#ifndef FLATHEADERS -#include "simple_message/shared_types.h" -#else -#include "shared_types.h" -#endif - -#include "string.h" - -namespace industrial -{ -namespace simple_serialize -{ -// Class declaration required for function prototypes below -class SimpleSerialize; -} -} - -namespace industrial -{ -namespace byte_array -{ - -/** - * \brief The byte array wraps a traditional, fixed size, array of bytes (i.e. char*). - * - * It provides convenient methods for loading and unloading - * data types to and from the byte array. The class acts as an - * interface definition to raw data (in case the underlying structure of the - * raw data changes). It's intended use is for socket communications. - * - * By default data using the load/unload methods is appended/removed from the end - * of the array. As long as the standard load/unload methods are uses, this is - * transparent to the user. - * - * A fixed size array is used for simplicity (i.e. avoiding re-implementing - * the STL vector class for those systems that don't have access to the STL, - * i.e. Motoman robotics Motoplus compiler. - * - * THIS CLASS IS NOT THREAD-SAFE - * - */ - -class ByteArray -{ -public: - - // Provides SimpleSerialize access to byte array internals - friend class SimpleSerialize; - - /** - * \brief Default constructor - * - * This method creates and empty byte array. - * - */ - ByteArray(void); - - /** - * \brief Destructor - * - */ - ~ByteArray(void); - - /** - * \brief Initializes or Reinitializes an empty buffer. - * - * This method sets all values to 0 and resets the buffer size. - * - */ - void init(); - - /** - * \brief initializes byte array from char* buffer - * - * This method creates a byte array containing a copy of the passed - * in buffer (up to byteSize) - * - * \param buffer pointer to byte buffer - * \param byte_size size of buffer to copy. If the byte size is greater - * than the max buffer size then only the max buffer amount of bytes is - * copied - * - * \return true on success, false otherwise (max array size exceeded). - */ - bool init(const char* buffer, const industrial::shared_types::shared_int byte_size); - - /** - * \brief Deep-Copy - * - * This method creates a byte array containing a deep copy of the - * passed in buffer - * - * \param buffer buffer to copy - * - */ - void copyFrom(ByteArray & buffer); - - /** - * \brief loads a boolean into the byte array - * - * \param value to load - * - * \return true on success, false otherwise (max array size exceeded). - * Value not loaded - */ - bool load(industrial::shared_types::shared_bool value); - - /** - * \brief loads a float on the byte array. If byte swapping is - * enabled, then the bytes are swapped (this assumes a common float - * representation) - * - * \param value to load - * - * \return true on success, false otherwise (max array size exceeded). - * Value not loaded - */ - bool load(industrial::shared_types::shared_real value); - - /** - * \brief loads an integer into the byte array. If byte swapping is - * enabled, then the bytes are swapped. - * - * \param value to load - * - * \return true on success, false otherwise (max array size exceeded). - * Value not loaded - */ - bool load(industrial::shared_types::shared_int value); - - /** - * \brief loads a complex SimpleSerialize into the byte array - * - * \param value to load - * - * \return true on success, false otherwise (max array size exceeded). - * Value not loaded - */ - bool load(industrial::simple_serialize::SimpleSerialize &value); - - /** - * \brief loads a whole byte array into this byte array - * - * \param value to load - * - * \return true on success, false otherwise (max array size exceeded). - * Value not loaded - */ - bool load(ByteArray &value); - - /** - * \brief loads a void* (treated as char*) into the byte array. - * WARNING: Byte swapping is not performed in this function. - * - * \param value to load - * \byte_syze number of bytes to load - * - * \return true on success, false otherwise (max array size exceeded). - * Value not loaded - */ - bool load(void* value, const industrial::shared_types::shared_int byte_size); - - /** - * \brief unloads a boolean value from the byte array - * - * \param value value to unload - * - * \return true on success, false otherwise (array is empty) - */ - bool unload(industrial::shared_types::shared_bool &value); - - /** - * \brief unloads a double value from the byte array. If byte swapping is - * enabled, then the bytes are swapped. - * - * \param value value to unload - * - * \return true on success, false otherwise (array is empty) - */ - bool unload(industrial::shared_types::shared_real &value); - - /** - * \brief unloads an integer value from the byte array. If byte swapping is - * enabled, then the bytes are swapped. - * - * \param value value to unload - * - * \return true on success, false otherwise (array is empty) - */ - bool unload(industrial::shared_types::shared_int &value); - - /** - * \brief unloads a complex SimpleSerialize value from the byte array - * - * \param value value to unload - * - * \return true on success, false otherwise (array is empty) - */ - bool unload(industrial::simple_serialize::SimpleSerialize &value); - - /** - * \brief unloads a partial byte array from the byte array into the - * passed in byte array (this is done using the byte array load method - * so any data in the passed in byte array remains intact) - * - * \param value value to unload - * - * \return true on success, false otherwise (array is empty) - */ - bool unload(ByteArray &value, const industrial::shared_types::shared_int byte_size); - - /** - * \brief unloads a void* (treated as char*) from the byte array. - * WARNING: Byte swapping is not performed in this function. - * - * \param value to unload - * \byte_syze number of bytes to unload - * - * \return true on success, false otherwise (array is empty) - */ - bool unload(void* value, const industrial::shared_types::shared_int byteSize); - - /** - * \brief unloads a double value from the beginning of the byte array. - * If byte swapping is enabled, then the bytes are swapped. - * WARNING: This method performs a memmove every time it is called (this is - * expensive). - * - * \param value value to unload - * - * \return true on success, false otherwise (array is empty) - */ - bool unloadFront(industrial::shared_types::shared_real &value); - - /** - * \brief unloads an integer value from the beginning of the byte array. - * If byte swapping is enabled, then the bytes are swapped - * WARNING: This method performs a memmove every time it is called (this is - * expensive). - * - * \param value value to unload - * - * \return true on success, false otherwise (array is empty) - */ - bool unloadFront(industrial::shared_types::shared_int &value); - - /** - * \brief unloads a void* (treated as char*) from the beginning of the array. - * WARNING: This method performs a memmove every time it is called (this is - * expensive). - * WARNING: Byte swapping is not performed in this function. - * - * \param value to unload - * \byte_syze number of bytes to unload - * - * \return true on success, false otherwise (array is empty) - */ - bool unloadFront(void* value, const industrial::shared_types::shared_int byteSize); - - /** - * \brief returns a char* pointer to the raw data. - * WARNING: This method is meant for read-only operations - * - * This function returns a pointer to the actual raw data stored within - * the class. Care should be taken not to modified the data oustide of - * the class. This method of providing a reference to private class data - * is used in order to avoid dynamic memory allocation that would be required - * to return a copy. - * - * \return char* pointer to the raw data - */ - char* getRawDataPtr(); - - /** - * \brief gets current buffer size - * - * \return buffer size - */ - unsigned int getBufferSize(); - - /** - * \brief gets current buffer size - * - * \return buffer size - */ - unsigned int getMaxBufferSize(); - - /** - * \brief returns true if byte swapping is enabled (this is a global - * option set by compiler flag). This function gives the status of the - * compiler flag. - * - * \return true if byte swapping is enabled. - */ - static bool isByteSwapEnabled(); - -private: - /** - * \brief maximum array size (WARNING: THIS VALUE SHOULD NOT EXCEED THE MAX - * 32-BIT INTEGER SIZE) - * - * The byte array class uses shared 32-bit types, so the buffer size cannot - * be larger than this. Ideally this value would be relatively small as passing - * large amounts of data is not desired. - */ - static const industrial::shared_types::shared_int MAX_SIZE = 1024; - - /** - * \brief internal data buffer - */ - char buffer_[MAX_SIZE]; - - /** - * \brief current buffer size - */ - industrial::shared_types::shared_int buffer_size_; - -#ifdef BYTE_SWAPPING - /** - * \brief Swaps byte of value (in place) - * - * \param value to swap - * \param byteSize (in bytes) - * - */ - void swap(void *value, industrial::shared_types::shared_int byteSize); -#endif - - /** - * \brief sets current buffer size - * - * \param size new size - * - * \return true on success, false otherwise (new size is too large) - */ - bool setBufferSize(const industrial::shared_types::shared_int size); - - /** - * \brief extends current buffer size - * - * \param number of bytes to extend - * - * \return true on success, false otherwise (new size is too large) - */ - bool extendBufferSize(const industrial::shared_types::shared_int size); - - /** - * \brief shortens current buffer size - * - * \param number of bytes to shorten - * - * \return true on success, false otherwise (new size is less than 0) - */ - bool shortenBufferSize(industrial::shared_types::shared_int size); - - /** - * \brief gets pointer to unload location (buffer_size + 1) - * - * \param size new size - * - * \return pointer to load location (NULL if array is at max size) - */ - char* getLoadPtr(); - - /** - * \brief gets pointer to unload location (buffer_size - num_bytes) - * - * The unload location is the beginning of the data item that is to - * be unloaded. - * - * \return pointer to load location (NULL is num_bytes > buffer_size_) - */ - char* getUnloadPtr(const industrial::shared_types::shared_int num_bytes); - -}; - -} // namespace industrial -} // namespace byte_array - -#endif //BYTE_ARRAY_H diff --git a/training/supplements/src/industrial_core/simple_message/include/simple_message/comms_fault_handler.h b/training/supplements/src/industrial_core/simple_message/include/simple_message/comms_fault_handler.h deleted file mode 100644 index 49fa945b..00000000 --- a/training/supplements/src/industrial_core/simple_message/include/simple_message/comms_fault_handler.h +++ /dev/null @@ -1,76 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2011, Southwest Research Institute - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in the - * documentation and/or other materials provided with the distribution. - * * Neither the name of the Southwest Research Institute, nor the names - * of its contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef COMMS_FAULT_HANDLER_H -#define COMMS_FAULT_HANDLER_H - -namespace industrial -{ -namespace comms_fault_handler -{ - -/** - * \brief Interface definition for communications fault handler. Defines the type - * of communcations faults that can be handled and the function callbacks that should - * be executed under the specific fault conditions. - * - */ -class CommsFaultHandler - -{ -public: - - /** - * \brief Send failure callback method. This method will be executed in the event - * that a comms send fails. - * - */ - virtual void sendFailCB()=0; - - /** - * \brief Receive failure callback method. This method will be executed in the event - * that a comms receive fails. - */ - virtual void receiveFailCB()=0; - - /** - * \brief Connection failure callback method. This method will be exectured in the - * event that a comms connection is lost. - * - */ - virtual void connectionFailCB()=0; - -}; - -} //namespace comms_fault_handler -} //namespace industrial - -#endif /* COMMS_FAULT_HANDLER_H */ diff --git a/training/supplements/src/industrial_core/simple_message/include/simple_message/joint_data.h b/training/supplements/src/industrial_core/simple_message/include/simple_message/joint_data.h deleted file mode 100644 index a625b983..00000000 --- a/training/supplements/src/industrial_core/simple_message/include/simple_message/joint_data.h +++ /dev/null @@ -1,168 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2011, Southwest Research Institute - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in the - * documentation and/or other materials provided with the distribution. - * * Neither the name of the Southwest Research Institute, nor the names - * of its contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef JOINT_DATA_H -#define JOINT_DATA_H - -#ifndef FLATHEADERS -#include "simple_message/simple_message.h" -#include "simple_message/simple_serialize.h" -#include "simple_message/shared_types.h" -#else -#include "simple_message.h" -#include "simple_serialize.h" -#include "shared_types.h" -#endif - -namespace industrial -{ -namespace joint_data -{ - -/** - * \brief Class encapsulated joint data (positions, accelerations, velocity, - * torque, and/or effort). - * - * For simplicity and cross platform compliance, this is implemented as a - * fixed size array. - * - * The byte representation of a joint data is as follows. The standard sizes - * are given, but can change based on type sizes: - * - * member: type size - * joints (industrial::shared_types::shared_real) 4 * MAX_NUM_JOINTS - * - * - * THIS CLASS IS NOT THREAD-SAFE - * - */ - -class JointData : public industrial::simple_serialize::SimpleSerialize -{ -public: - /** - * \brief Default constructor - * - * This method creates empty data. - * - */ - JointData(void); - /** - * \brief Destructor - * - */ - ~JointData(void); - - /** - * \brief Initializes a empty joint data - * - */ - void init(); - - /** - * \brief Sets a joint value within the buffer - * - * \param joint index - * \param joint value - * - * \return true if value set, otherwise false (index greater than max) - */ - bool setJoint(industrial::shared_types::shared_int index, industrial::shared_types::shared_real value); - - /** - * \brief Gets a joint value within the buffer - * - * \param joint index - * \param joint value (passed by reference) - * - * \return true if value valid, otherwise false (index greater than max) - */ - bool getJoint(industrial::shared_types::shared_int index, industrial::shared_types::shared_real & value) const; - - /** - * \brief Gets a joint value within the buffer (Only use this form if you are - * sure the index is within bounds). - * - * \param joint index - * - * \return joint value (returns 0.0 if index is out of bounds) - */ - industrial::shared_types::shared_real getJoint(industrial::shared_types::shared_int index) const; - - /** - * \brief Copies the passed in value - * - * \param src (value to copy) - */ - void copyFrom(JointData &src); - - /** - * \brief returns the maximum number of joints the message holds - * - * \return max number of joints - */ - int getMaxNumJoints() const - { - return MAX_NUM_JOINTS; - } - - /** - * \brief == operator implementation - * - * \return true if equal - */ - bool operator==(JointData &rhs); - - // Overrides - SimpleSerialize - bool load(industrial::byte_array::ByteArray *buffer); - bool unload(industrial::byte_array::ByteArray *buffer); - unsigned int byteLength() - { - return MAX_NUM_JOINTS * sizeof(industrial::shared_types::shared_real); - } - -private: - - /** - * \brief maximum number of joints positions that can be held in the message. - */ - static const industrial::shared_types::shared_int MAX_NUM_JOINTS = 10; - /** - * \brief internal data buffer - */ - industrial::shared_types::shared_real joints_[MAX_NUM_JOINTS]; - -}; - -} -} - -#endif /* JOINT_DATA_H */ diff --git a/training/supplements/src/industrial_core/simple_message/include/simple_message/joint_feedback.h b/training/supplements/src/industrial_core/simple_message/include/simple_message/joint_feedback.h deleted file mode 100644 index c96b9f33..00000000 --- a/training/supplements/src/industrial_core/simple_message/include/simple_message/joint_feedback.h +++ /dev/null @@ -1,334 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2013, Southwest Research Institute - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in the - * documentation and/or other materials provided with the distribution. - * * Neither the name of the Southwest Research Institute, nor the names - * of its contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef JOINT_FEEDBACK_H -#define JOINT_FEEDBACK_H - -#ifndef FLATHEADERS -#include "simple_message/joint_data.h" -#include "simple_message/simple_message.h" -#include "simple_message/simple_serialize.h" -#include "simple_message/shared_types.h" -#else -#include "joint_data.h" -#include "simple_message.h" -#include "simple_serialize.h" -#include "shared_types.h" -#endif - -namespace industrial -{ -namespace joint_feedback -{ - -namespace ValidFieldTypes -{ -enum ValidFieldType -{ - TIME = 0x01, POSITION = 0x02, VELOCITY = 0x04, ACCELERATION = 0x08 -}; -} -typedef ValidFieldTypes::ValidFieldType ValidFieldType; - -/** - * \brief Class encapsulated joint feedback data. This data represents the - * current state of each robot joint, including position/velocity/acceleration. - * The specific interpretation of this data (actual vs. commanded, timestamp, etc.) - * is up to the robot-controller implementation. - * - * The message data-packet byte representation is as follows (ordered lowest index - * to highest). The standard sizes are given, but can change based on type sizes: - * - * member: type size - * robot_id (industrial::shared_types::shared_int) 4 bytes - * valid_fields (industrial::shared_types::shared_int) 4 bytes - * time (industrial::shared_types::shared_real) 4 bytes - * positions (industrial::joint_data) 40 bytes - * velocities (industrial::joint_data) 40 bytes - * accelerations (industrial::joint_data) 40 bytes - * - * - * THIS CLASS IS NOT THREAD-SAFE - * - */ - -class JointFeedback : public industrial::simple_serialize::SimpleSerialize -{ -public: - - /** - * \brief Default constructor - * - * This method creates empty data. - * - */ - JointFeedback(void); - /** - * \brief Destructor - * - */ - ~JointFeedback(void); - - /** - * \brief Initializes a empty joint feedback - * - */ - void init(); - - /** - * \brief Initializes a complete joint feedback - * - */ - void init(industrial::shared_types::shared_int robot_id, - industrial::shared_types::shared_int valid_fields, - industrial::shared_types::shared_real time, - industrial::joint_data::JointData & positions, - industrial::joint_data::JointData & velocities, - industrial::joint_data::JointData & accelerations); - - /** - * \brief Sets robot_id. - * Robot group # (0-based), for controllers with multiple axis-groups. - * - * \param robot_id new robot_id value - */ - void setRobotID(industrial::shared_types::shared_int robot_id) - { - this->robot_id_ = robot_id; - } - - /** - * \brief Gets robot_id. - * Robot group # (0-based), for controllers with multiple axis-groups. - * - * @return robot_id value - */ - industrial::shared_types::shared_int getRobotID() - { - return this->robot_id_; - } - - /** - * \brief Sets joint feedback timestamp - * - * \param time new time value - */ - void setTime(industrial::shared_types::shared_real time) - { - this->time_ = time; - this->valid_fields_ |= ValidFieldTypes::TIME; // set the bit - } - - /** - * \brief Returns joint feedback timestamp - * - * \param time returned time value - * \return true if this field contains valid data - */ - bool getTime(industrial::shared_types::shared_real & time) - { - time = this->time_; - return is_valid(ValidFieldTypes::TIME); - } - - /** - * \brief Clears the joint feedback timestamp - */ - void clearTime() - { - this->time_ = 0; - this->valid_fields_ &= ~ValidFieldTypes::TIME; // clear the bit - } - - /** - * \brief Sets joint position data - * - * \param positions new joint position data - */ - void setPositions(industrial::joint_data::JointData &positions) - { - this->positions_.copyFrom(positions); - this->valid_fields_ |= ValidFieldTypes::POSITION; // set the bit - } - - /** - * \brief Returns a copy of the position data - * - * \param dest returned joint position - * \return true if this field contains valid data - */ - bool getPositions(industrial::joint_data::JointData &dest) - { - dest.copyFrom(this->positions_); - return is_valid(ValidFieldTypes::POSITION); - } - - /** - * \brief Clears the position data - */ - void clearPositions() - { - this->positions_.init(); - this->valid_fields_ &= ~ValidFieldTypes::POSITION; // clear the bit - } - - /** - * \brief Sets joint velocity data - * - * \param velocities new joint velocity data - */ - void setVelocities(industrial::joint_data::JointData &velocities) - { - this->velocities_.copyFrom(velocities); - this->valid_fields_ |= ValidFieldTypes::VELOCITY; // set the bit - } - - /** - * \brief Returns a copy of the velocity data - * - * \param dest returned joint velocity - * \return true if this field contains valid data - */ - bool getVelocities(industrial::joint_data::JointData &dest) - { - dest.copyFrom(this->velocities_); - return is_valid(ValidFieldTypes::VELOCITY); - } - - /** - * \brief Clears the velocity data - */ - void clearVelocities() - { - this->velocities_.init(); - this->valid_fields_ &= ~ValidFieldTypes::VELOCITY; // clear the bit - } - /** - * \brief Sets joint acceleration data - * - * \param accelerations new joint acceleration data - */ - void setAccelerations(industrial::joint_data::JointData &accelerations) - { - this->accelerations_.copyFrom(accelerations); - this->valid_fields_ |= ValidFieldTypes::ACCELERATION; // set the bit - } - - /** - * \brief Returns a copy of the acceleration data - * - * \param dest returned joint acceleration - * \return true if this field contains valid data - */ - bool getAccelerations(industrial::joint_data::JointData &dest) - { - dest.copyFrom(this->accelerations_); - return is_valid(ValidFieldTypes::ACCELERATION); - } - - /** - * \brief Clears the acceleration data - */ - void clearAccelerations() - { - this->accelerations_.init(); - this->valid_fields_ &= ~ValidFieldTypes::ACCELERATION; // clear the bit - } - - - /** - * \brief Copies the passed in value - * - * \param src (value to copy) - */ - void copyFrom(JointFeedback &src); - - /** - * \brief == operator implementation - * - * \return true if equal - */ - bool operator==(JointFeedback &rhs); - - /** - * \brief check the validity state for a given field - * @param field field to check - * @return true if specified field contains valid data - */ - bool is_valid(ValidFieldType field) - { - return valid_fields_ & field; - } - - // Overrides - SimpleSerialize - bool load(industrial::byte_array::ByteArray *buffer); - bool unload(industrial::byte_array::ByteArray *buffer); - unsigned int byteLength() - { - return 2*sizeof(industrial::shared_types::shared_int) + sizeof(industrial::shared_types::shared_real) - + 3*this->positions_.byteLength(); - } - -private: - - /** - * \brief robot group # (0-based) for controllers that support multiple axis-groups - */ - industrial::shared_types::shared_int robot_id_; - /** - * \brief bit-mask of (optional) fields that have been initialized with valid data - * \see enum ValidFieldTypes - */ - industrial::shared_types::shared_int valid_fields_; - /** - * \brief joint data timestamp - * Typically, time since controller booted (in seconds) - */ - industrial::shared_types::shared_real time_; - - /** - * \brief joint feedback positional data - */ - industrial::joint_data::JointData positions_; - /** - * \brief joint feedback velocity data - */ - industrial::joint_data::JointData velocities_; /** - * \brief joint feedback acceleration data - */ - industrial::joint_data::JointData accelerations_; - -}; - -} -} - -#endif /* JOINT_FEEDBACK_H */ diff --git a/training/supplements/src/industrial_core/simple_message/include/simple_message/joint_traj.h b/training/supplements/src/industrial_core/simple_message/include/simple_message/joint_traj.h deleted file mode 100644 index bbbe3fa5..00000000 --- a/training/supplements/src/industrial_core/simple_message/include/simple_message/joint_traj.h +++ /dev/null @@ -1,184 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2011, Southwest Research Institute - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in the - * documentation and/or other materials provided with the distribution. - * * Neither the name of the Southwest Research Institute, nor the names - * of its contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef JOINT_TRAJ_H -#define JOINT_TRAJ_H - -#ifndef FLATHEADERS -#include "simple_message/simple_message.h" -#include "simple_message/simple_serialize.h" -#include "simple_message/shared_types.h" -#include "simple_message/joint_traj_pt.h" -#else -#include "simple_message.h" -#include "simple_serialize.h" -#include "shared_types.h" -#include "joint_traj_pt.h" -#endif - - - -namespace industrial -{ -namespace joint_traj -{ - -/** - * \brief Class encapsulated joint trajectory. A joint trajectory includes - * an array of JointTrajPt data types. The intention for this class is to - * be loaded into a single message for communication over a simple connection. - * - * For simplicity and cross platform compliance, this is implemented as a - * fixed size array. The size of the trajectory cannot exceed the max size - * of the array. - */ -//* JointTraj -/** - * - * THIS CLASS IS NOT THREAD-SAFE - * - */ - -class JointTraj : public industrial::simple_serialize::SimpleSerialize -{ -public: - /** - * \brief Default constructor - * - * This method creates empty data. - * - */ - JointTraj(void); - /** - * \brief Destructor - * - */ - ~JointTraj(void); - - /** - * \brief Initializes a empty joint data - * - */ - void init(); - - /** - * \brief Adds a point value to the end of the buffer - * - * \param point value - * - * \return true if value set, otherwise false (buffer is full) - */ - bool addPoint(industrial::joint_traj_pt::JointTrajPt & point); - - /** - * \brief Gets a point value within the buffer - * - * \param point index - * \param point value - * - * \return true if value set, otherwise false (index greater than size) - */ - bool getPoint(industrial::shared_types::shared_int index, - industrial::joint_traj_pt::JointTrajPt & point); - /** - * \brief Gets a size of trajectory - * - * \return trajectory size - */ - industrial::shared_types::shared_int size() - { - return this->size_; - } - - /** - * \brief returns True if buffer is full - * - * \return true if buffer is full - */ - bool isFull() - { - return this->size_ >= this->getMaxNumPoints(); - } - - /** - * \brief returns the maximum number of points the message holds - * - * \return max number of points - */ - int getMaxNumPoints() const - { - return MAX_NUM_POINTS; - } - - /** - * \brief Copies the passed in value - * - * \param src (value to copy) - */ - void copyFrom(JointTraj &src); - - /** - * \brief == operator implementation - * - * \return true if equal - */ - bool operator==(JointTraj &rhs); - - // Overrides - SimpleSerialize - bool load(industrial::byte_array::ByteArray *buffer); - bool unload(industrial::byte_array::ByteArray *buffer); - unsigned int byteLength() - { - industrial::joint_traj_pt::JointTrajPt pt; - return this->size() * pt.byteLength(); - } - -private: - - /** - * \brief maximum number of joints positions that can be held in the message. - */ - static const industrial::shared_types::shared_int MAX_NUM_POINTS = 200; - /** - * \brief internal data buffer - */ - industrial::joint_traj_pt::JointTrajPt points_[MAX_NUM_POINTS]; - /** - * \brief size of trajectory - */ - industrial::shared_types::shared_int size_; - -}; - -} -} - -#endif /* JOINT_TRAJ_H */ diff --git a/training/supplements/src/industrial_core/simple_message/include/simple_message/joint_traj_pt.h b/training/supplements/src/industrial_core/simple_message/include/simple_message/joint_traj_pt.h deleted file mode 100644 index 2ed01fe5..00000000 --- a/training/supplements/src/industrial_core/simple_message/include/simple_message/joint_traj_pt.h +++ /dev/null @@ -1,245 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2011, Southwest Research Institute - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in the - * documentation and/or other materials provided with the distribution. - * * Neither the name of the Southwest Research Institute, nor the names - * of its contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef JOINT_TRAJ_PT_H -#define JOINT_TRAJ_PT_H - -#ifndef FLATHEADERS -#include "simple_message/joint_data.h" -#include "simple_message/simple_message.h" -#include "simple_message/simple_serialize.h" -#include "simple_message/shared_types.h" -#else -#include "joint_data.h" -#include "simple_message.h" -#include "simple_serialize.h" -#include "shared_types.h" -#endif - -namespace industrial -{ -namespace joint_traj_pt -{ - -namespace SpecialSeqValues -{ -enum SpecialSeqValue -{ - START_TRAJECTORY_DOWNLOAD = -1, START_TRAJECOTRY_STREAMING = -2, END_TRAJECTORY = -3, STOP_TRAJECTORY = -4 -}; -} -typedef SpecialSeqValues::SpecialSeqValue SpecialSeqValue; - -/** - * \brief Class encapsulated joint trajectory point data. The point data - * serves as a waypoint along a trajectory and is meant to mirror the - * JointTrajectoryPoint message. - * - * This point differs from the ROS trajectory point in the following ways: - * - * - The joint velocity in an industrial robot standard way (as a single value). - * - The duration is somewhat different than the ROS timestamp. The timestamp - * specifies when the move should start, where as the duration is how long the - * move should take. A big assumption is that a sequence of points is continuously - * executed. This is generally true of a ROS trajectory but not required. - * - * The byte representation of a joint trajectory point is as follow (in order lowest index - * to highest). The standard sizes are given, but can change based on type sizes: - * - * member: type size - * sequence (industrial::shared_types::shared_int) 4 bytes - * joints (industrial::joint_data) 40 bytes - * velocity (industrial::shared_types::shared_real) 4 bytes - * duration (industrial::shared_types::shared_real) 4 bytes - * - * - * THIS CLASS IS NOT THREAD-SAFE - * - */ - -class JointTrajPt : public industrial::simple_serialize::SimpleSerialize -{ -public: - /** - * \brief Default constructor - * - * This method creates empty data. - * - */ - JointTrajPt(void); - /** - * \brief Destructor - * - */ - ~JointTrajPt(void); - - /** - * \brief Initializes a empty joint trajectory point - * - */ - void init(); - - /** - * \brief Initializes a complete trajectory point - * - */ - void init(industrial::shared_types::shared_int sequence, industrial::joint_data::JointData & position, - industrial::shared_types::shared_real velocity, industrial::shared_types::shared_real duration); - - /** - * \brief Sets joint position data - * - * \param joint position data - */ - void setJointPosition(industrial::joint_data::JointData &position) - { - this->joint_position_.copyFrom(position); - } - - /** - * \brief Returns a copy of the position data - * - * \param joint position dest - */ - void getJointPosition(industrial::joint_data::JointData &dest) - { - dest.copyFrom(this->joint_position_); - } - - /** - * \brief Sets joint trajectory point sequence number - * - * \param sequence value - */ - void setSequence(industrial::shared_types::shared_int sequence) - { - this->sequence_ = sequence; - } - - /** - * \brief Returns joint trajectory point sequence number - * - * \return joint trajectory sequence number - */ - industrial::shared_types::shared_int getSequence() - { - return this->sequence_; - } - - /** - * \brief Sets joint trajectory point velocity - * - * \param velocity value - */ - void setVelocity(industrial::shared_types::shared_real velocity) - { - this->velocity_ = velocity; - } - - /** - * \brief Returns joint trajectory point velocity - * - * \return joint trajectory velocity - */ - industrial::shared_types::shared_real getVelocity() - { - return this->velocity_; - } - - /** - * \brief Sets joint trajectory point duration - * - * \param velocity value - */ - void setDuration(industrial::shared_types::shared_real duration) - { - this->duration_ = duration; - } - - /** - * \brief Returns joint trajectory point duration - * - * \return joint trajectory velocity - */ - industrial::shared_types::shared_real getDuration() - { - return this->duration_; - } - - /** - * \brief Copies the passed in value - * - * \param src (value to copy) - */ - void copyFrom(JointTrajPt &src); - - /** - * \brief == operator implementation - * - * \return true if equal - */ - bool operator==(JointTrajPt &rhs); - - // Overrides - SimpleSerialize - bool load(industrial::byte_array::ByteArray *buffer); - bool unload(industrial::byte_array::ByteArray *buffer); - unsigned int byteLength() - { - return sizeof(industrial::shared_types::shared_real) + sizeof(industrial::shared_types::shared_int) - + this->joint_position_.byteLength(); - } - -private: - - /** - * \brief joint point positional data - */ - industrial::joint_data::JointData joint_position_; - /** - * \brief joint point velocity - */ - industrial::shared_types::shared_real velocity_; - /** - * \brief trajectory sequence number - */ - industrial::shared_types::shared_int sequence_; - - /** - * \brief joint move duration - */ - industrial::shared_types::shared_real duration_; - -}; - -} -} - -#endif /* JOINT_TRAJ_PT_H */ diff --git a/training/supplements/src/industrial_core/simple_message/include/simple_message/joint_traj_pt_full.h b/training/supplements/src/industrial_core/simple_message/include/simple_message/joint_traj_pt_full.h deleted file mode 100644 index 155f282d..00000000 --- a/training/supplements/src/industrial_core/simple_message/include/simple_message/joint_traj_pt_full.h +++ /dev/null @@ -1,372 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2013, Southwest Research Institute - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in the - * documentation and/or other materials provided with the distribution. - * * Neither the name of the Southwest Research Institute, nor the names - * of its contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef JOINT_TRAJ_PT_FULL_H -#define JOINT_TRAJ_PT_FULL_H - -#ifndef FLATHEADERS -#include "simple_message/joint_data.h" -#include "simple_message/simple_message.h" -#include "simple_message/simple_serialize.h" -#include "simple_message/shared_types.h" -#else -#include "joint_data.h" -#include "simple_message.h" -#include "simple_serialize.h" -#include "shared_types.h" -#endif - -namespace industrial -{ -namespace joint_traj_pt_full -{ - -namespace SpecialSeqValues -{ -enum SpecialSeqValue -{ - START_TRAJECTORY_DOWNLOAD = -1, START_TRAJECOTRY_STREAMING = -2, END_TRAJECTORY = -3, STOP_TRAJECTORY = -4 -}; -} -typedef SpecialSeqValues::SpecialSeqValue SpecialSeqValue; - -namespace ValidFieldTypes -{ -enum ValidFieldType -{ - TIME = 0x01, POSITION = 0x02, VELOCITY = 0x04, ACCELERATION = 0x08 -}; -} -typedef ValidFieldTypes::ValidFieldType ValidFieldType; - -/** - * \brief Class encapsulated joint trajectory point data. The point data - * serves as a waypoint along a trajectory and is meant to mirror the - * JointTrajectoryPoint message. - * - * This class is similar to the simple_message joint_traj_pt class, but this - * class provides the full message contents directly to the robot controller, - * rather than simplifying the velocity duration. - * - * The message data-packet byte representation is as follows (ordered lowest index - * to highest). The standard sizes are given, but can change based on type sizes: - * - * member: type size - * robot_id (industrial::shared_types::shared_int) 4 bytes - * sequence (industrial::shared_types::shared_int) 4 bytes - * valid_fields (industrial::shared_types::shared_int) 4 bytes - * time (industrial::shared_types::shared_real) 4 bytes - * positions (industrial::joint_data) 40 bytes - * velocities (industrial::joint_data) 40 bytes - * accelerations (industrial::joint_data) 40 bytes - * - * - * THIS CLASS IS NOT THREAD-SAFE - * - */ - -class JointTrajPtFull : public industrial::simple_serialize::SimpleSerialize -{ -public: - - /** - * \brief Default constructor - * - * This method creates empty data. - * - */ - JointTrajPtFull(void); - /** - * \brief Destructor - * - */ - ~JointTrajPtFull(void); - - /** - * \brief Initializes a empty joint trajectory point - * - */ - void init(); - - /** - * \brief Initializes a complete trajectory point - * - */ - void init(industrial::shared_types::shared_int robot_id, - industrial::shared_types::shared_int sequence, - industrial::shared_types::shared_int valid_fields, - industrial::shared_types::shared_real time, - industrial::joint_data::JointData & positions, - industrial::joint_data::JointData & velocities, - industrial::joint_data::JointData & accelerations); - - /** - * \brief Sets robot_id. - * Robot group # (0-based), for controllers with multiple axis-groups. - * - * \param robot_id new robot_id value - */ - void setRobotID(industrial::shared_types::shared_int robot_id) - { - this->robot_id_ = robot_id; - } - - /** - * \brief Gets robot_id. - * Robot group # (0-based), for controllers with multiple axis-groups. - * - * @return robot_id value - */ - industrial::shared_types::shared_int getRobotID() - { - return this->robot_id_; - } - - /** - * \brief Sets joint trajectory point sequence number - * - * \param sequence value - */ - void setSequence(industrial::shared_types::shared_int sequence) - { - this->sequence_ = sequence; - } - - /** - * \brief Returns joint trajectory point sequence number - * - * \return joint trajectory sequence number - */ - industrial::shared_types::shared_int getSequence() - { - return this->sequence_; - } - - /** - * \brief Sets joint trajectory point timestamp - * - * \param time new time value - */ - void setTime(industrial::shared_types::shared_real time) - { - this->time_ = time; - this->valid_fields_ |= ValidFieldTypes::TIME; // set the bit - } - - /** - * \brief Returns joint trajectory point timestamp - * - * \param time returned time value - * \return true if this field contains valid data - */ - bool getTime(industrial::shared_types::shared_real & time) - { - time = this->time_; - return is_valid(ValidFieldTypes::TIME); - } - - /** - * \brief Clears the joint trajectory point timestamp - */ - void clearTime() - { - this->time_ = 0; - this->valid_fields_ &= ~ValidFieldTypes::TIME; // clear the bit - } - - /** - * \brief Sets joint position data - * - * \param positions new joint position data - */ - void setPositions(industrial::joint_data::JointData &positions) - { - this->positions_.copyFrom(positions); - this->valid_fields_ |= ValidFieldTypes::POSITION; // set the bit - } - - /** - * \brief Returns a copy of the position data - * - * \param dest returned joint position - * \return true if this field contains valid data - */ - bool getPositions(industrial::joint_data::JointData &dest) - { - dest.copyFrom(this->positions_); - return is_valid(ValidFieldTypes::POSITION); - } - - /** - * \brief Clears the position data - */ - void clearPositions() - { - this->positions_.init(); - this->valid_fields_ &= ~ValidFieldTypes::POSITION; // clear the bit - } - - /** - * \brief Sets joint velocity data - * - * \param velocities new joint velocity data - */ - void setVelocities(industrial::joint_data::JointData &velocities) - { - this->velocities_.copyFrom(velocities); - this->valid_fields_ |= ValidFieldTypes::VELOCITY; // set the bit - } - - /** - * \brief Returns a copy of the velocity data - * - * \param dest returned joint velocity - * \return true if this field contains valid data - */ - bool getVelocities(industrial::joint_data::JointData &dest) - { - dest.copyFrom(this->velocities_); - return is_valid(ValidFieldTypes::VELOCITY); - } - - /** - * \brief Clears the velocity data - */ - void clearVelocities() - { - this->velocities_.init(); - this->valid_fields_ &= ~ValidFieldTypes::VELOCITY; // clear the bit - } - /** - * \brief Sets joint acceleration data - * - * \param accelerations new joint acceleration data - */ - void setAccelerations(industrial::joint_data::JointData &accelerations) - { - this->accelerations_.copyFrom(accelerations); - this->valid_fields_ |= ValidFieldTypes::ACCELERATION; // set the bit - } - - /** - * \brief Returns a copy of the acceleration data - * - * \param dest returned joint acceleration - * \return true if this field contains valid data - */ - bool getAccelerations(industrial::joint_data::JointData &dest) - { - dest.copyFrom(this->accelerations_); - return is_valid(ValidFieldTypes::ACCELERATION); - } - - /** - * \brief Clears the acceleration data - */ - void clearAccelerations() - { - this->accelerations_.init(); - this->valid_fields_ &= ~ValidFieldTypes::ACCELERATION; // clear the bit - } - - - /** - * \brief Copies the passed in value - * - * \param src (value to copy) - */ - void copyFrom(JointTrajPtFull &src); - - /** - * \brief == operator implementation - * - * \return true if equal - */ - bool operator==(JointTrajPtFull &rhs); - - /** - * \brief check the validity state for a given field - * @param field field to check - * @return true if specified field contains valid data - */ - bool is_valid(ValidFieldType field) - { - return valid_fields_ & field; - } - - // Overrides - SimpleSerialize - bool load(industrial::byte_array::ByteArray *buffer); - bool unload(industrial::byte_array::ByteArray *buffer); - unsigned int byteLength() - { - return 3*sizeof(industrial::shared_types::shared_int) + sizeof(industrial::shared_types::shared_real) - + 3*this->positions_.byteLength(); - } - -private: - - /** - * \brief robot group # (0-based) for controllers that support multiple axis-groups - */ - industrial::shared_types::shared_int robot_id_; - /** - * \brief trajectory sequence number - */ - industrial::shared_types::shared_int sequence_; - /** - * \brief bit-mask of (optional) fields that have been initialized with valid data - * \see enum ValidFieldTypes - */ - industrial::shared_types::shared_int valid_fields_; - /** - * \brief joint trajectory point timestamp - * Typically, time_from_start of this trajectory (in seconds) - */ - industrial::shared_types::shared_real time_; - - /** - * \brief joint trajectory point positional data - */ - industrial::joint_data::JointData positions_; - /** - * \brief joint trajectory point velocity data - */ - industrial::joint_data::JointData velocities_; /** - * \brief joint trajectory point acceleration data - */ - industrial::joint_data::JointData accelerations_; - -}; - -} -} - -#endif /* JOINT_TRAJ_PT_FULL_H */ diff --git a/training/supplements/src/industrial_core/simple_message/include/simple_message/log_wrapper.h b/training/supplements/src/industrial_core/simple_message/include/simple_message/log_wrapper.h deleted file mode 100644 index 466b9aa5..00000000 --- a/training/supplements/src/industrial_core/simple_message/include/simple_message/log_wrapper.h +++ /dev/null @@ -1,118 +0,0 @@ -/* -* Software License Agreement (BSD License) -* -* Copyright (c) 2011, Southwest Research Institute -* All rights reserved. -* -* Redistribution and use in source and binary forms, with or without -* modification, are permitted provided that the following conditions are met: -* -* * Redistributions of source code must retain the above copyright -* notice, this list of conditions and the following disclaimer. -* * Redistributions in binary form must reproduce the above copyright -* notice, this list of conditions and the following disclaimer in the -* documentation and/or other materials provided with the distribution. -* * Neither the name of the Southwest Research Institute, nor the names -* of its contributors may be used to endorse or promote products derived -* from this software without specific prior written permission. -* -* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE -* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE -* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR -* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF -* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS -* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN -* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) -* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -* POSSIBILITY OF SUCH DAMAGE. -*/ - -#ifndef LOG_WRAPPER_H_ -#define LOG_WRAPPER_H_ - -#ifdef ROS -#include "ros/ros.h" -#endif - -#ifdef MOTOPLUS -#include "motoPlus.h" -#endif - -namespace industrial -{ - -/** - * \brief Contains macro that wrap standard logging calls. Wrapping logging - * calls allows libraries to be used inside and outside the ROS framework. - * - * Macros are used because passing variable argument lists are much easier - * than passing them through functions. - */ -namespace log_wrapper -{ - - -// Define ROS if this library will execute under ROS -#ifdef ROS - -// The LOG_COMM redirects to debug in ROS because ROS has -// debug filtering tools that allow the communications messages -// to be easily removed from the logs -#define LOG_COMM(format, ...) \ - ROS_DEBUG(format, ##__VA_ARGS__) - -#define LOG_DEBUG(format, ...) \ - ROS_DEBUG(format, ##__VA_ARGS__) - -#define LOG_INFO(format, ...) \ - ROS_INFO(format, ##__VA_ARGS__) - -#define LOG_WARN(format, ...) \ - ROS_WARN(format, ##__VA_ARGS__) - -#define LOG_ERROR(format, ...) \ - ROS_ERROR(format, ##__VA_ARGS__) - -#define LOG_FATAL(format, ...) \ - ROS_FATAL(FATAL, ##__VA_ARGS__) - -#elif defined(STDIOLOG) - -#define LOG(level, format, ...) \ -do \ -{ \ - printf(level); \ - printf(": "); \ - printf(format, ##__VA_ARGS__); \ - printf("\n"); \ - } while (0) - -// WARNING: LOG_COMM produces many messages and could slow down program -// execution on the robot. -#define LOG_COMM(format, ...) LOG("COMM", format, ##__VA_ARGS__) -#define LOG_DEBUG(format, ...) LOG("DEBUG", format, ##__VA_ARGS__) -#define LOG_INFO(format, ...) LOG("INFO", format, ##__VA_ARGS__) -#define LOG_WARN(format, ...) LOG("WARNING", format, ##__VA_ARGS__) -#define LOG_ERROR(format, ...) LOG("ERROR", format, ##__VA_ARGS__) -#define LOG_FATAL(format, ...) LOG("FATAL", format, ##__VA_ARGS__) - -#else -// LOG DISABLED - -#define LOG_COMM(format, ...) -#define LOG_DEBUG(format, ...) -#define LOG_INFO(format, ...) -#define LOG_WARN(format, ...) -#define LOG_ERROR(format, ...) -#define LOG_FATAL(format, ...) - -#endif - - - -} // namespace industrial -} // namespace loge_wrapper - -#endif /* LOG_WRAPPER_H_ */ diff --git a/training/supplements/src/industrial_core/simple_message/include/simple_message/message_handler.h b/training/supplements/src/industrial_core/simple_message/include/simple_message/message_handler.h deleted file mode 100644 index 1d63f5ff..00000000 --- a/training/supplements/src/industrial_core/simple_message/include/simple_message/message_handler.h +++ /dev/null @@ -1,178 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2011, Southwest Research Institute - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in the - * documentation and/or other materials provided with the distribution. - * * Neither the name of the Southwest Research Institute, nor the names - * of its contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef MESSAGE_HANDLER_H -#define MESSAGE_HANDLER_H - -#ifndef FLATHEADERS -#include "simple_message/simple_message.h" -#include "simple_message/smpl_msg_connection.h" -#else -#include "simple_message.h" -#include "smpl_msg_connection.h" -#endif - -namespace industrial -{ -namespace message_handler -{ - -/** - * \brief Interface definition for message handlers. The interface defines the - * callback function that should execute when a message is received. - */ -//* MessageHandler -/** - * Defines the interface used for function callbacks when a message is received. - * When used in conjunction with a message_manager (link) generic message handling - * can be achieved. - * - * THIS CLASS IS NOT THREAD-SAFE - * - */ -class MessageHandler - -{ -public: - - /** - * \brief Constructor - */ - MessageHandler(); - - /** - * \brief Destructor - */ - ~MessageHandler(); - - /** - * \brief Callback function that should be executed when a message arrives - * DO NOT OVERRIDE THIS FUNCTION. It performs message validation before the - * internal callback (which should be overridden) is called. If one is required - * the callback sends a message reply - * - * \param in incoming message - * - * \return true on success, false otherwise - */ - bool callback(industrial::simple_message::SimpleMessage & in); - - /** - * \brief Gets message type that callback expects - * - * \return message type - */ - int getMsgType() - { - return this->msg_type_; - } - ; - -protected: - /** - * \brief Gets connectoin for message replies - * - * \return connection reference - */ - industrial::smpl_msg_connection::SmplMsgConnection*getConnection() - { - return this->connection_; - } - ; - - /** - * \brief Class initializer - * - * \param msg_type type of message expected - * \param connection simple message connection that will be used to send replies. - * - * \return true on success, false otherwise (an invalid message type) - */ - bool init(int msg_type, industrial::smpl_msg_connection::SmplMsgConnection* connection); - -private: - - /** - * \brief Reference to reply connection (called if incoming message requires a reply) - */ - industrial::smpl_msg_connection::SmplMsgConnection* connection_; - - /** - * \brief Message type expected by callback - */ - int msg_type_; - - /** - * \brief Virtual callback function - * - * \param in incoming message - * - * \return true on success, false otherwise - */ - virtual bool internalCB(industrial::simple_message::SimpleMessage & in)=0; - - /** - * \brief Validates incoming message for processing by internal callback - * - * \param in incoming message - * - * \return true on if valid, false otherwise - */ - bool validateMsg(industrial::simple_message::SimpleMessage & in); - - /** - * \brief Sets connection for message replies - * - * \param connection connection reference - */ - void setConnection(industrial::smpl_msg_connection::SmplMsgConnection* connection) - { - this->connection_ = connection; - } - ; - - /** - * \brief Sets message type that callback expects - * - * \param msg_type message type - */ - void setMsgType(int msg_type) - { - this->msg_type_ = msg_type; - } - ; - -}; - -} //namespace message_handler -} //namespace industrial - -#endif //MESSAGE_HANDLER_H diff --git a/training/supplements/src/industrial_core/simple_message/include/simple_message/message_manager.h b/training/supplements/src/industrial_core/simple_message/include/simple_message/message_manager.h deleted file mode 100644 index 78abc302..00000000 --- a/training/supplements/src/industrial_core/simple_message/include/simple_message/message_manager.h +++ /dev/null @@ -1,295 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2011, Southwest Research Institute - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in the - * documentation and/or other materials provided with the distribution. - * * Neither the name of the Southwest Research Institute, nor the names - * of its contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef MESSAGE_MANAGER_H -#define MESSAGE_MANAGER_H - -#ifndef FLATHEADERS -#include "simple_message/smpl_msg_connection.h" -#include "simple_message/message_handler.h" -#include "simple_message/ping_handler.h" -#include "simple_message/comms_fault_handler.h" -#include "simple_message/simple_comms_fault_handler.h" -#else -#include "smpl_msg_connection.h" -#include "message_handler.h" -#include "ping_handler.h" -#include "comms_fault_handler.h" -#include "simple_comms_fault_handler.h" -#endif - - -namespace industrial -{ -namespace message_manager -{ - -/** - * \brief The message manager handles communications for a simple server. - */ -//* MessageManager -/** - * The message manager receives simple messages via it's communications connection. - * It then performs function callbacks based on the message type. Callbacks perform - * the desired operation and send a reply, if required. - * - * The message manager can be run in two ways. spin(), similar to ROS this executes - * a blocking execution indefinitely or spinOnce(), where a single execution of the - * manager is performed. In spinOnce mode, other server operations can be made, but - * the server application must make certain to execute the spinOnce() function at a - * minimum rate so as not to loose connection data. - * - * THIS CLASS IS NOT THREAD-SAFE - * - */ - -class MessageManager -{ - -public: - - /** - * \brief Constructor - */ - MessageManager(); - - /** - * \brief Destructor - */ - ~MessageManager(); - - /** - * \brief Class initializer - * - * \param connection simple message connection that will be managed. - * This connection must be properly initialized and connected before - * it is passed to the manager. - * - * \return true on success, false otherwise - */ - bool init(industrial::smpl_msg_connection::SmplMsgConnection* connection); - - /** - * \brief Class initializer - * - * \param simple message connection that will be managed. - * This connection must be properly initialized and connected before - * it is passed to the manager. - * - * \param connection fault handler to be used in case of a connection - * fault. - * - * \return true on success, false otherwise - */ - bool init(industrial::smpl_msg_connection::SmplMsgConnection* connection, - industrial::comms_fault_handler::CommsFaultHandler* fault_handler); - - /** - * \brief Perform an single execution of the message manager (a single - * receive and send (if required) - */ - void spinOnce(); - - /** - * \brief Perform a indefinite execution of the message manager - */ - void spin(); - - /** - * \brief Adds a message handler to the manager - * - * \param handler handler to add - * \param replace existing handler (of same msg-type), if exists - * - * \return true if successful, otherwise false (max # of handlers reached) - */ - bool add(industrial::message_handler::MessageHandler* handler, bool allow_replace = false); - - /** - * \brief Gets number of handlers - * - * \return connection reference - */ - unsigned int getNumHandlers() - { - return this->num_handlers_; - } - - /** - * \brief Gets maximumn number of handlers - * - * \return connection reference - */ - unsigned int getMaxNumHandlers() - { - return this->MAX_NUM_HANDLERS; - } - - - /** - * \brief Gets communications fault handler - * - * \return reference to message handler or NULL if one doesn't exist - */ - industrial::comms_fault_handler::CommsFaultHandler* getCommsFaultHandler() - { - return this->comms_hndlr_; - } - - /** - * \brief Gets communications fault handler - * - * \param Pointer to message handler - */ - void setCommsFaultHandler(industrial::comms_fault_handler::CommsFaultHandler* handler) - { - this->comms_hndlr_ = handler; - } - - -private: - - /** - * \brief Maximum number of handlers - * - * The number of handlers is limited in order to avoid dynamic memory allocation - * on robot controllers. - */ - static const unsigned int MAX_NUM_HANDLERS = 64; - - /** - * \brief buffer of handlers - */ - industrial::message_handler::MessageHandler* handlers_[MAX_NUM_HANDLERS]; - - /** - * \brief Reference to reply connection (called if incoming message requires a reply) - */ - industrial::smpl_msg_connection::SmplMsgConnection* connection_; - - /** - * \brief Internal ping handle (by default every message manager can handle pings) - */ - industrial::ping_handler::PingHandler ping_hndlr_; - - /** - * \brief Internal default comms handler (this is used if a communications fault handler is - * not specified as part of the class init. - */ - industrial::simple_comms_fault_handler::SimpleCommsFaultHandler def_comms_hndlr_; - - /** - * \brief Reference to comms handler - */ - industrial::comms_fault_handler::CommsFaultHandler* comms_hndlr_; - - /** - * \brief Number of handlers - */ - unsigned int num_handlers_; - - /** - * \brief Gets message handler for specific message type - * - * \param msg_type message type to handle - * - * \return reference to message handler or NULL if one doesn't exist - */ - industrial::message_handler::MessageHandler* getHandler(int msg_type); - - /** - * \brief Gets index of message handler for specific message type - * - * \param message type to handle - * - * \return index of matching handler or -1 if one doesn't exist - */ - int getHandlerIdx(int msg_type); - - /** - * \brief Gets default communications fault handler - * - * \return reference to message handler or NULL if one doesn't exist - */ - industrial::simple_comms_fault_handler::SimpleCommsFaultHandler& getDefaultCommsFaultHandler() - { - return this->def_comms_hndlr_; - } - /** - * \brief Gets ping handler - * - * \return connection reference - */ - industrial::ping_handler::PingHandler& getPingHandler() - { - return this->ping_hndlr_; - } - ; - - /** - * \brief Sets connection manager - * - * \param connection connection reference - */ - void setConnection(industrial::smpl_msg_connection::SmplMsgConnection* connection) - { - this->connection_ = connection; - } - ; - - /** - * \brief Gets connection for manager - * - * \return connection reference - */ - industrial::smpl_msg_connection::SmplMsgConnection* getConnection() - { - return this->connection_; - } - ; - - /** - * \brief Sets message type that callback expects - * - * \param msg_type message type - */ - void setNumHandlers(unsigned int num_handlers) - { - this->num_handlers_ = num_handlers; - } - ; - -}; - -} // namespace industrial -} // namespace message_manager - -#endif //MESSAGE_MANAGER diff --git a/training/supplements/src/industrial_core/simple_message/include/simple_message/messages/joint_feedback_message.h b/training/supplements/src/industrial_core/simple_message/include/simple_message/messages/joint_feedback_message.h deleted file mode 100644 index 25ffeb62..00000000 --- a/training/supplements/src/industrial_core/simple_message/include/simple_message/messages/joint_feedback_message.h +++ /dev/null @@ -1,147 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2013, Southwest Research Institute - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in the - * documentation and/or other materials provided with the distribution. - * * Neither the name of the Southwest Research Institute, nor the names - * of its contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef JOINT_FEEDBACK_MESSAGE_H -#define JOINT_FEEDBACK_MESSAGE_H - -#ifndef FLATHEADERS -#include "simple_message/typed_message.h" -#include "simple_message/simple_message.h" -#include "simple_message/shared_types.h" -#include "simple_message/joint_feedback.h" -#else -#include "typed_message.h" -#include "simple_message.h" -#include "shared_types.h" -#include "joint_feedback.h" -#endif - -namespace industrial -{ -namespace joint_feedback_message -{ - - -/** - * \brief Class encapsulated joint feedback message generation methods - * (either to or from a industrial::simple_message::SimpleMessage type. - * - * This message simply wraps the industrial::joint_feedback::JointFeedback data type. - * The data portion of this typed message matches JointFeedback. - * - * - * THIS CLASS IS NOT THREAD-SAFE - * - */ - -class JointFeedbackMessage : public industrial::typed_message::TypedMessage - -{ -public: - /** - * \brief Default constructor - * - * This method creates an empty message. - * - */ - JointFeedbackMessage(void); - /** - * \brief Destructor - * - */ - ~JointFeedbackMessage(void); - /** - * \brief Initializes message from a simple message - * - * \param simple message to construct from - * - * \return true if message successfully initialized, otherwise false - */ - bool init(industrial::simple_message::SimpleMessage & msg); - - /** - * \brief Initializes message from a joint feedback structure - * - * \param joint feedback data structure - * - */ - void init(industrial::joint_feedback::JointFeedback & data); - - /** - * \brief Initializes a new message - * - */ - void init(); - - // Overrides - SimpleSerialize - bool load(industrial::byte_array::ByteArray *buffer); - bool unload(industrial::byte_array::ByteArray *buffer); - - unsigned int byteLength() - { - return this->data_.byteLength(); - } - - industrial::shared_types::shared_int getRobotID() - { - return this->data_.getRobotID(); - } - - bool getTime(industrial::shared_types::shared_real & time) - { - return this->data_.getTime(time); - } - - bool getPositions(industrial::joint_data::JointData &dest) - { - return this->data_.getPositions(dest); - } - - bool getVelocities(industrial::joint_data::JointData &dest) - { - return this->data_.getVelocities(dest); - } - - bool getAccelerations(industrial::joint_data::JointData &dest) - { - return this->data_.getAccelerations(dest); - } - -private: - - industrial::joint_feedback::JointFeedback data_; - -}; - -} -} - -#endif /* JOINT_FEEDBACK_MESSAGE_H */ diff --git a/training/supplements/src/industrial_core/simple_message/include/simple_message/messages/joint_message.h b/training/supplements/src/industrial_core/simple_message/include/simple_message/messages/joint_message.h deleted file mode 100644 index fc29b890..00000000 --- a/training/supplements/src/industrial_core/simple_message/include/simple_message/messages/joint_message.h +++ /dev/null @@ -1,175 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2011, Southwest Research Institute - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in the - * documentation and/or other materials provided with the distribution. - * * Neither the name of the Southwest Research Institute, nor the names - * of its contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef JOINT_MESSAGE_H -#define JOINT_MESSAGE_H - -#ifndef FLATHEADERS -#include "simple_message/typed_message.h" -#include "simple_message/simple_message.h" -#include "simple_message/shared_types.h" -#include "simple_message/joint_data.h" -#else -#include "typed_message.h" -#include "simple_message.h" -#include "shared_types.h" -#include "joint_data.h" -#endif - -namespace industrial -{ -namespace joint_message -{ - -/** - * \brief Enumeration of special sequence values that signal the end of trajectory - * or an immediate stop. - */ -namespace SpecialSeqValues -{ -enum SpecialSeqValue -{ - END_TRAJECTORY = -1, STOP_TRAJECTORY = -2 -}; -} -typedef SpecialSeqValues::SpecialSeqValue SpecialSeqValue; - -/** - * \brief Class encapsulated joint message generation methods (either to or - * from a SimpleMessage type. This message represents the joint position data. - * NOTE: In earlier versions this was simply referred to as JOINT message. This - * caused confusion as there are many types of joint messages (position, velocity, - * feedback). To remove confusion, this message was changed to JOINT_POSITION. - * Other types of messages will have to be created for velocity and other feedback. - * - * The byte representation of a joint message is as follow (in order lowest index - * to highest). The standard sizes are given, but can change based on type sizes: - * - * member: type size - * sequence (industrial::shared_types::shared_int) 4 bytes - * joints (industrial::joint_data) 40 bytes - * - * - * THIS CLASS IS NOT THREAD-SAFE - * - */ - -class JointMessage : public industrial::typed_message::TypedMessage -{ -public: - /** - * \brief Default constructor - * - * This method creates an empty message. - * - */ - JointMessage(void); - /** - * \brief Destructor - * - */ - ~JointMessage(void); - /** - * \brief Initializes message from a simple message - * - * \param simple message to construct from - * - * \return true if message successfully initialized, otherwise false - */ - bool init(industrial::simple_message::SimpleMessage & msg); - - /** - * \brief Initializes message from a joint structure - * - * \param sequence number - * \param joints - * - */ - void init(industrial::shared_types::shared_int seq, industrial::joint_data::JointData & joints); - - /** - * \brief Initializes a new joint message - * - */ - void init(); - - /** - * \brief Sets message sequence number - * - * \param message sequence number - */ - void setSequence(industrial::shared_types::shared_int sequence); - - /** - * \brief returns the maximum message sequence number - * - * \return message sequence number - */ - industrial::shared_types::shared_int getSequence() - { - return sequence_; - } - - /** - * \brief returns reference to underlying joint class - * - * \return reference to joint class - */ - industrial::joint_data::JointData& getJoints() - { - return this->joints_; - } - - // Overrides - SimpleSerialize - bool load(industrial::byte_array::ByteArray *buffer); - bool unload(industrial::byte_array::ByteArray *buffer); - - unsigned int byteLength() - { - return sizeof(industrial::shared_types::shared_int) + this->joints_.byteLength(); - } - -private: - /** - * \brief sequence number (for those joints messages that require it) - */ - industrial::shared_types::shared_int sequence_; - /** - * \brief maximum number of joints positions that can be held in the message. - */ - industrial::joint_data::JointData joints_; - -}; - -} -} - -#endif /* JOINT_MESSAGE_H */ diff --git a/training/supplements/src/industrial_core/simple_message/include/simple_message/messages/joint_traj_pt_full_message.h b/training/supplements/src/industrial_core/simple_message/include/simple_message/messages/joint_traj_pt_full_message.h deleted file mode 100644 index e21ee7cb..00000000 --- a/training/supplements/src/industrial_core/simple_message/include/simple_message/messages/joint_traj_pt_full_message.h +++ /dev/null @@ -1,130 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2013, Southwest Research Institute - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in the - * documentation and/or other materials provided with the distribution. - * * Neither the name of the Southwest Research Institute, nor the names - * of its contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef JOINT_TRAJ_PT_FULL_MESSAGE_H -#define JOINT_TRAJ_PT_FULL_MESSAGE_H - -#ifndef FLATHEADERS -#include "simple_message/typed_message.h" -#include "simple_message/simple_message.h" -#include "simple_message/shared_types.h" -#include "simple_message/joint_traj_pt_full.h" -#else -#include "typed_message.h" -#include "simple_message.h" -#include "shared_types.h" -#include "joint_traj_pt_full.h" -#endif - -namespace industrial -{ -namespace joint_traj_pt_full_message -{ - - -/** - * \brief Class encapsulated joint trajectory point message generation methods - * (either to or from a industrial::simple_message::SimpleMessage type. - * - * This message simply wraps the industrial::joint_traj_pt_full::JointTrajPtFull data type. - * The data portion of this typed message matches JointTrajPtFull. - * - * - * THIS CLASS IS NOT THREAD-SAFE - * - */ - -class JointTrajPtFullMessage : public industrial::typed_message::TypedMessage - -{ -public: - /** - * \brief Default constructor - * - * This method creates an empty message. - * - */ - JointTrajPtFullMessage(void); - /** - * \brief Destructor - * - */ - ~JointTrajPtFullMessage(void); - /** - * \brief Initializes message from a simple message - * - * \param simple message to construct from - * - * \return true if message successfully initialized, otherwise false - */ - bool init(industrial::simple_message::SimpleMessage & msg); - - /** - * \brief Initializes message from a joint trajectory point structure - * - * \param joint trajectory point data structure - * - */ - void init(industrial::joint_traj_pt_full::JointTrajPtFull & point); - - /** - * \brief Initializes a new message - * - */ - void init(); - - // Overrides - SimpleSerialize - bool load(industrial::byte_array::ByteArray *buffer); - bool unload(industrial::byte_array::ByteArray *buffer); - - unsigned int byteLength() - { - return this->point_.byteLength(); - } - - /** - * \brief Sets message sequence number - * - * \param message sequence number - */ - void setSequence(industrial::shared_types::shared_int sequence) { point_.setSequence(sequence); } - - industrial::joint_traj_pt_full::JointTrajPtFull point_; - -private: - - -}; - -} -} - -#endif /* JOINT_TRAJ_PT_FULL_MESSAGE_H */ diff --git a/training/supplements/src/industrial_core/simple_message/include/simple_message/messages/joint_traj_pt_message.h b/training/supplements/src/industrial_core/simple_message/include/simple_message/messages/joint_traj_pt_message.h deleted file mode 100644 index ec6fd18f..00000000 --- a/training/supplements/src/industrial_core/simple_message/include/simple_message/messages/joint_traj_pt_message.h +++ /dev/null @@ -1,130 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2011, Southwest Research Institute - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in the - * documentation and/or other materials provided with the distribution. - * * Neither the name of the Southwest Research Institute, nor the names - * of its contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef JOINT_TRAJ_PT_MESSAGE_H -#define JOINT_TRAJ_PT_MESSAGE_H - -#ifndef FLATHEADERS -#include "simple_message/typed_message.h" -#include "simple_message/simple_message.h" -#include "simple_message/shared_types.h" -#include "simple_message/joint_traj_pt.h" -#else -#include "typed_message.h" -#include "simple_message.h" -#include "shared_types.h" -#include "joint_traj_pt.h" -#endif - -namespace industrial -{ -namespace joint_traj_pt_message -{ - - -/** - * \brief Class encapsulated joint trajectory point message generation methods - * (either to or from a industrial::simple_message::SimpleMessage type. - * - * This message simply wraps the industrial::joint_traj_pt::JointTrajPt data type. - * The data portion of this typed message matches JointTrajPt. - * - * - * THIS CLASS IS NOT THREAD-SAFE - * - */ - -class JointTrajPtMessage : public industrial::typed_message::TypedMessage - -{ -public: - /** - * \brief Default constructor - * - * This method creates an empty message. - * - */ - JointTrajPtMessage(void); - /** - * \brief Destructor - * - */ - ~JointTrajPtMessage(void); - /** - * \brief Initializes message from a simple message - * - * \param simple message to construct from - * - * \return true if message successfully initialized, otherwise false - */ - bool init(industrial::simple_message::SimpleMessage & msg); - - /** - * \brief Initializes message from a joint trajectory point structure - * - * \param joint trajectory point data structure - * - */ - void init(industrial::joint_traj_pt::JointTrajPt & point); - - /** - * \brief Initializes a new message - * - */ - void init(); - - // Overrides - SimpleSerialize - bool load(industrial::byte_array::ByteArray *buffer); - bool unload(industrial::byte_array::ByteArray *buffer); - - unsigned int byteLength() - { - return this->point_.byteLength(); - } - - /** - * \brief Sets message sequence number - * - * \param message sequence number - */ - void setSequence(industrial::shared_types::shared_int sequence) { point_.setSequence(sequence); } - - industrial::joint_traj_pt::JointTrajPt point_; - -private: - - -}; - -} -} - -#endif /* JOINT_TRAJ_PT_MESSAGE_H */ diff --git a/training/supplements/src/industrial_core/simple_message/include/simple_message/messages/robot_status_message.h b/training/supplements/src/industrial_core/simple_message/include/simple_message/messages/robot_status_message.h deleted file mode 100644 index 74717164..00000000 --- a/training/supplements/src/industrial_core/simple_message/include/simple_message/messages/robot_status_message.h +++ /dev/null @@ -1,122 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2012, Southwest Research Institute - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in the - * documentation and/or other materials provided with the distribution. - * * Neither the name of the Southwest Research Institute, nor the names - * of its contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef ROBOT_STATUS_MESSAGE_H -#define ROBOT_STATUS_MESSAGE_H - -#ifndef FLATHEADERS -#include "simple_message/typed_message.h" -#include "simple_message/simple_message.h" -#include "simple_message/shared_types.h" -#include "simple_message/robot_status.h" -#else -#include "typed_message.h" -#include "simple_message.h" -#include "shared_types.h" -#include "robot_status.h" -#endif - -namespace industrial -{ -namespace robot_status_message -{ - - - -/** - * \brief Class encapsulated robot status message generation methods - * (either to or from a industrial::simple_message::SimpleMessage type. - * - * This message simply wraps the industrial::robot_status::RobotStatus data type. - * The data portion of this typed message matches RobotStatus. - * - * - * THIS CLASS IS NOT THREAD-SAFE - * - */ - -class RobotStatusMessage : public industrial::typed_message::TypedMessage -{ -public: - /** - * \brief Default constructor - * - * This method creates an empty message. - * - */ - RobotStatusMessage(void); - /** - * \brief Destructor - * - */ - ~RobotStatusMessage(void); - /** - * \brief Initializes message from a simple message - * - * \param simple message to construct from - * - * \return true if message successfully initialized, otherwise false - */ - bool init(industrial::simple_message::SimpleMessage & msg); - - /** - * \brief Initializes message from a robot status structure - * - * \param status strcutre to initialize from - * - */ - void init(industrial::robot_status::RobotStatus & status); - - /** - * \brief Initializes a new robot status message - * - */ - void init(); - - - // Overrides - SimpleSerialize - bool load(industrial::byte_array::ByteArray *buffer); - bool unload(industrial::byte_array::ByteArray *buffer); - - unsigned int byteLength() - { - return this->status_.byteLength(); - } - - industrial::robot_status::RobotStatus status_; - - -}; - -} -} - -#endif /* JOINT_MESSAGE_H */ diff --git a/training/supplements/src/industrial_core/simple_message/include/simple_message/ping_handler.h b/training/supplements/src/industrial_core/simple_message/include/simple_message/ping_handler.h deleted file mode 100644 index b2603258..00000000 --- a/training/supplements/src/industrial_core/simple_message/include/simple_message/ping_handler.h +++ /dev/null @@ -1,103 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2011, Southwest Research Institute - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in the - * documentation and/or other materials provided with the distribution. - * * Neither the name of the Southwest Research Institute, nor the names - * of its contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - - -#ifndef PING_HANDLER_H -#define PING_HANDLER_H - -#ifndef FLATHEADERS -#include "simple_message/message_handler.h" -#else -#include "message_handler.h" -#endif - - -namespace industrial -{ -namespace ping_handler -{ - -/** - * \brief Message handler that handles ping messages. - */ -//* MessageHandler -/** - * Responds to ping message types. A ping is a simple message that is meant to - * test communications channels. A ping simply responds with a copy of the data - * it was sent. - * - * THIS CLASS IS NOT THREAD-SAFE - * - */ -class PingHandler : public industrial::message_handler::MessageHandler -{ - -public: - /** -* \brief Class initializer -* -* \param connection simple message connection that will be used to send replies. -* -* \return true on success, false otherwise (an invalid message type) -*/ -bool init(industrial::smpl_msg_connection::SmplMsgConnection* connection); - - /** -* \brief Class initializer (Direct call to base class with the same name) -* I couldn't get the "using" form to work/ -* -* \param connection simple message connection that will be used to send replies. -* -* \return true on success, false otherwise (an invalid message type) -*/ -bool init(int msg_type, industrial::smpl_msg_connection::SmplMsgConnection* connection) -{ return MessageHandler::init(msg_type, connection);}; - - -private: - - - - /** - * \brief Callback executed upon receiving a ping message - * - * \param in incoming message - * - * \return true on success, false otherwise - */ - bool internalCB(industrial::simple_message::SimpleMessage & in); -}; - -}//ping_handler -}//industrial - - -#endif /* PING_HANDLER_H_ */ diff --git a/training/supplements/src/industrial_core/simple_message/include/simple_message/ping_message.h b/training/supplements/src/industrial_core/simple_message/include/simple_message/ping_message.h deleted file mode 100644 index a9128ccd..00000000 --- a/training/supplements/src/industrial_core/simple_message/include/simple_message/ping_message.h +++ /dev/null @@ -1,114 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2011, Southwest Research Institute - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in the - * documentation and/or other materials provided with the distribution. - * * Neither the name of the Southwest Research Institute, nor the names - * of its contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef PING_MESSAGE_H -#define PING_MESSAGE_H - -#ifndef FLATHEADERS -#include "simple_message/typed_message.h" -#include "simple_message/simple_message.h" -#else -#include "typed_message.h" -#include "simple_message.h" -#endif - - -namespace industrial -{ -namespace ping_message -{ - -/** - * \brief Class encapsulated ping message generation methods (either to or - * from a SimpleMessage type. - */ -//* PingMessage -/** - * - * THIS CLASS IS NOT THREAD-SAFE - * - */ - -class PingMessage : public industrial::typed_message::TypedMessage -{ -public: - - /** - * \brief Default constructor - * - * This method creates an empty byte ping message. - * - */ - PingMessage(void); - - /** - * \brief Destructor - * - */ - ~PingMessage(void); - - /** - * \brief Initializes message from a simple message - * - * \return true if message successfully initialized, otherwise false - */ - bool init(industrial::simple_message::SimpleMessage & msg); - - /** - * \brief Initializes a new ping message - * - */ - void init(); - - /** - * \brief The ping message overrides the base method toTopic to always - * return false. A ping cannot be sent as a topic. - * - */ - bool toTopic(industrial::simple_message::SimpleMessage & msg) - { - return false; - } - - // Overrides - SimpleSerialize - bool load(industrial::byte_array::ByteArray *buffer){return true;} - bool unload(industrial::byte_array::ByteArray *buffer){return true;} - unsigned int byteLength(){return 0;} - -private: - - -}; - -} -} - -#endif /* PING_MESSAGE_H */ diff --git a/training/supplements/src/industrial_core/simple_message/include/simple_message/robot_status.h b/training/supplements/src/industrial_core/simple_message/include/simple_message/robot_status.h deleted file mode 100644 index 4a94a04f..00000000 --- a/training/supplements/src/industrial_core/simple_message/include/simple_message/robot_status.h +++ /dev/null @@ -1,279 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2011, Southwest Research Institute - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in the - * documentation and/or other materials provided with the distribution. - * * Neither the name of the Southwest Research Institute, nor the names - * of its contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef ROBOT_STATUS_H -#define ROBOT_STATUS_H - -#ifndef FLATHEADERS -#include "simple_message/simple_message.h" -#include "simple_message/simple_serialize.h" -#include "simple_message/shared_types.h" -#else -#include "simple_message.h" -#include "simple_serialize.h" -#include "shared_types.h" -#endif - -namespace industrial -{ -namespace robot_status -{ - -/** - * \brief Enumeration mirrors industrial_msgs/RobotMode definition - * - */ -namespace RobotModes -{ -enum RobotMode -{ - UNKNOWN = -1, - - MANUAL = 1, AUTO = 2, -}; - -#ifdef ROS -int toROSMsgEnum(RobotModes::RobotMode mode); -#endif - -} -typedef RobotModes::RobotMode RobotMode; - -/** - * \brief Enumeration mirrors industrial_msgs/TriState definition. - * NOTE: The TS prefix is needed because the ON and TRUE value collide - * with other defined types on some systems. - * - */ -namespace TriStates -{ - -enum TriState -{ - TS_UNKNOWN = -1, - // These values must all be the same - TS_TRUE = 1, TS_ON = 1, TS_ENABLED = 1, TS_HIGH = 1, - // These values must all be the same - TS_FALSE = 0, TS_OFF = 0, TS_DISABLED = 0, TS_LOW = 0 -}; - -#ifdef ROS -int toROSMsgEnum(TriStates::TriState state); -#endif - -} -typedef TriStates::TriState TriState; - -/** - * \brief Class encapsulated robot status data. The robot status data is - * meant to mirror the industrial_msgs/RobotStatus message. - * - * - * The byte representation of a robot status is as follows (in order lowest index - * to highest). The standard sizes are given, but can change based on type sizes: - * - * member: type size - * drives_powered (industrial::shared_types::shared_int) 4 bytes - * e_stopped (industrial::shared_types::shared_int) 4 bytes - * error_code (industrial::shared_types::shared_int) 4 bytes - * in_error (industrial::shared_types::shared_int) 4 bytes - * in_motion (industrial::shared_types::shared_int) 4 bytes - * mode (industrial::shared_types::shared_int) 4 bytes - * motion_possible (industrial::shared_types::shared_int) 4 bytes - * - * THIS CLASS IS NOT THREAD-SAFE - * - */ - -class RobotStatus : public industrial::simple_serialize::SimpleSerialize -{ -public: -/** - * \brief Default constructor - * - * This method creates empty data. - * - */ -RobotStatus(void); -/** - * \brief Destructor - * - */ -~RobotStatus(void); - -/** - * \brief Initializes an empty robot status - * - */ -void init(); - -/** - * \brief Initializes a full robot status message - * - */ -void init(TriState drivesPowered, TriState eStopped, industrial::shared_types::shared_int errorCode, TriState inError, - TriState inMotion, RobotMode mode, TriState motionPossible); - -TriState getDrivesPowered() -{ - return TriState(drives_powered_); -} - -TriState getEStopped() -{ - return TriState(e_stopped_); -} - -industrial::shared_types::shared_int getErrorCode() const -{ - return error_code_; -} - -TriState getInError() -{ - return TriState(in_error_); -} - -TriState getInMotion() -{ - return TriState(in_motion_); -} - -RobotMode getMode() -{ - return RobotMode(mode_); -} - -TriState getMotionPossible() -{ - return TriState(motion_possible_); -} - -void setDrivesPowered(TriState drivesPowered) -{ - this->drives_powered_ = drivesPowered; -} - -void setEStopped(TriState eStopped) -{ - this->e_stopped_ = eStopped; -} - -void setErrorCode(industrial::shared_types::shared_int errorCode) -{ - this->error_code_ = errorCode; -} - -void setInError(TriState inError) -{ - this->in_error_ = inError; -} - -void setInMotion(TriState inMotion) -{ - this->in_motion_ = inMotion; -} - -void setMode(RobotMode mode) -{ - this->mode_ = mode; -} - -void setMotionPossible(TriState motionPossible) -{ - this->motion_possible_ = motionPossible; -} - -/** - * \brief Copies the passed in value - * - * \param src (value to copy) - */ -void copyFrom(RobotStatus &src); - -/** - * \brief == operator implementation - * - * \return true if equal - */ -bool operator==(RobotStatus &rhs); - -// Overrides - SimpleSerialize -bool load(industrial::byte_array::ByteArray *buffer); -bool unload(industrial::byte_array::ByteArray *buffer); -unsigned int byteLength() -{ - return 7 * sizeof(industrial::shared_types::shared_int); -} - -private: - -/** - * \brief Operating mode (see RobotModes::RobotMode) - */ -industrial::shared_types::shared_int mode_; - -/** - * \brief E-stop state (see TriStates::TriState) - */ -industrial::shared_types::shared_int e_stopped_; - -/** - * \brief Drive power state (see TriStates::TriState) - */ -industrial::shared_types::shared_int drives_powered_; - -/** - * \brief motion possible state (see TriStates::TriState) - */ -industrial::shared_types::shared_int motion_possible_; - -/** - * \brief in motion state (see TriStates::TriState) - */ -industrial::shared_types::shared_int in_motion_; - -/** - * \brief in error state (see TriStates::TriState) - */ -industrial::shared_types::shared_int in_error_; - -/** - * \brief error code (non-zero is error) - */ -industrial::shared_types::shared_int error_code_; - -}; - -} -} - -#endif /* JOINT_TRAJ_PT_H */ diff --git a/training/supplements/src/industrial_core/simple_message/include/simple_message/shared_types.h b/training/supplements/src/industrial_core/simple_message/include/simple_message/shared_types.h deleted file mode 100644 index d7c35eb2..00000000 --- a/training/supplements/src/industrial_core/simple_message/include/simple_message/shared_types.h +++ /dev/null @@ -1,77 +0,0 @@ -/* -* Software License Agreement (BSD License) -* -* Copyright (c) 2011, Southwest Research Institute -* All rights reserved. -* -* Redistribution and use in source and binary forms, with or without -* modification, are permitted provided that the following conditions are met: -* -* * Redistributions of source code must retain the above copyright -* notice, this list of conditions and the following disclaimer. -* * Redistributions in binary form must reproduce the above copyright -* notice, this list of conditions and the following disclaimer in the -* documentation and/or other materials provided with the distribution. -* * Neither the name of the Southwest Research Institute, nor the names -* of its contributors may be used to endorse or promote products derived -* from this software without specific prior written permission. -* -* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE -* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE -* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR -* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF -* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS -* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN -* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) -* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -* POSSIBILITY OF SUCH DAMAGE. -*/ - -#ifndef SHARED_TYPES_H_ -#define SHARED_TYPES_H_ - -namespace industrial -{ - -/** - * \brief Contains platform specific type definitions that guarantee the size - * of primitive data types. - * - * The byte size of shared data types was determined by balancing the needs to - * represent large data values with that of the platforms limits. In the case - * of platform limits, robot controllers represent the most restrictive limits. - * - * The majority of robot controllers have 32-bit architectures. As such this was - * chosen as the base for communications. Real and Integer data types are represented - * as 4 bytes. - * - * All types must match size and structure of the ROS_TYPES - */ -namespace shared_types -{ - -#if defined(INT32) -#include "stdint.h" -typedef int32_t shared_int; -#elif defined(INT64) -#include "stdint.h" -typedef int64_t shared_int; -#else -typedef int shared_int; -#endif - -#ifndef FLOAT64 -typedef float shared_real; -#else -typedef double shared_real; -#endif - -typedef bool shared_bool; - - -} // namespace shared_types -} // namespace industrial - -#endif /* SHARED_TYPES_H */ diff --git a/training/supplements/src/industrial_core/simple_message/include/simple_message/simple_comms_fault_handler.h b/training/supplements/src/industrial_core/simple_message/include/simple_message/simple_comms_fault_handler.h deleted file mode 100644 index b53cc737..00000000 --- a/training/supplements/src/industrial_core/simple_message/include/simple_message/simple_comms_fault_handler.h +++ /dev/null @@ -1,135 +0,0 @@ -/* -* Software License Agreement (BSD License) -* -* Copyright (c) 2011, Southwest Research Institute -* All rights reserved. -* -* Redistribution and use in source and binary forms, with or without -* modification, are permitted provided that the following conditions are met: -* -* * Redistributions of source code must retain the above copyright -* notice, this list of conditions and the following disclaimer. -* * Redistributions in binary form must reproduce the above copyright -* notice, this list of conditions and the following disclaimer in the -* documentation and/or other materials provided with the distribution. -* * Neither the name of the Southwest Research Institute, nor the names -* of its contributors may be used to endorse or promote products derived -* from this software without specific prior written permission. -* -* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE -* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE -* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR -* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF -* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS -* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN -* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) -* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -* POSSIBILITY OF SUCH DAMAGE. -*/ - -#ifndef DEFAULT_COMMS_FAULT_HANDLER_H -#define DEFAULT_COMMS_FAULT_HANDLER_H - -#ifndef FLATHEADERS -#include "simple_message/comms_fault_handler.h" -#include "simple_message/smpl_msg_connection.h" -#include "simple_message/log_wrapper.h" -#else -#include "comms_fault_handler.h" -#include "smpl_msg_connection.h" -#include "log_wrapper.h" -#endif - -namespace industrial -{ -namespace simple_comms_fault_handler -{ - -/** - * \brief Default implementation of comms fault handler. This class attempts - * to reconnect if the connection is lost. - * - * THIS CLASS IS NOT THREAD-SAFE - * - */ -class SimpleCommsFaultHandler : public industrial::comms_fault_handler::CommsFaultHandler - -{ -public: - - /** - * \brief Default constructor - * - */ - SimpleCommsFaultHandler(); - - /** - * \brief Destructor - * - */ - ~SimpleCommsFaultHandler(); - /** - * \brief Initializes default communications fault handler - * - * \param message connection to use for reconnecting - * - * \return true on success, false otherwise - */ - bool init(industrial::smpl_msg_connection::SmplMsgConnection* connection); - - - /** - * \brief Send failure callback method: Nothing is performed - * - */ - void sendFailCB() {LOG_WARN("Send failure, no callback support");}; - - /** - * \brief Receive failure callback method: Nothing is performed - * - */ - void receiveFailCB() {LOG_WARN("Receive failure, no callback support");}; - - /** - * \brief Connection failure callback method: On a connection failure - * a blocking reconnection is attempted. - * - */ - void connectionFailCB(); - -private: - - -/** - * \brief Reference to reply connection (called if incoming message requires a reply) - */ -industrial::smpl_msg_connection::SmplMsgConnection* connection_; - -/** - * \brief Sets connection manager - * - * \param connection connection reference - */ - void setConnection(industrial::smpl_msg_connection::SmplMsgConnection* connection) - { - this->connection_ = connection; - } - ; - - /** - * \brief Gets connection for manager - * - * \return connection reference - */ - industrial::smpl_msg_connection::SmplMsgConnection* getConnection() - { - return this->connection_; - } -}; - -} //namespace default_comms_fault_handler -} //namespace industrial - -#endif /* DEFAULT_COMMS_FAULT_HANDLER_H */ diff --git a/training/supplements/src/industrial_core/simple_message/include/simple_message/simple_message.h b/training/supplements/src/industrial_core/simple_message/include/simple_message/simple_message.h deleted file mode 100755 index a40a16f3..00000000 --- a/training/supplements/src/industrial_core/simple_message/include/simple_message/simple_message.h +++ /dev/null @@ -1,348 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2011, Southwest Research Institute - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in the - * documentation and/or other materials provided with the distribution. - * * Neither the name of the Southwest Research Institute, nor the names - * of its contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef SIMPLE_MSG_H -#define SIMPLE_MSG_H - -#ifndef FLATHEADERS -#include "simple_message/simple_serialize.h" -#include "simple_message/byte_array.h" -#include "simple_message/shared_types.h" -#else -#include "simple_serialize.h" -#include "byte_array.h" -#include "shared_types.h" -#endif - - -namespace industrial -{ - -namespace simple_message -{ - -/** - * \brief Enumeration of standard message types (supported by all platforms). - * In addition, each robot interface will support it's own message types. - */ -namespace StandardMsgTypes -{ - enum StandardMsgType - { - INVALID = 0, - PING = 1, - - //TODO: Keeping these message type for the time being. Refactoring - // the messages should remove the need for this message. - JOINT_POSITION = 10, - JOINT = 10, - READ_INPUT = 20, - WRITE_OUTPUT = 21, - - JOINT_TRAJ_PT = 11, //Joint trajectory point message (typically for streaming) - JOINT_TRAJ = 12, //Joint trajectory message (typically for trajectory downloading) - STATUS = 13, //Robot status message (for reporting the robot state) - JOINT_TRAJ_PT_FULL = 14, // Joint trajectory point message (all message fields) - JOINT_FEEDBACK = 15, // Feedback of joint pos/vel/accel - - // Begin vendor specific message types (only define the beginning enum value, - // specific enum values should be defined locally, within in the range reserved - // here. Each vendor can reserve up 1000 types - - SWRI_MSG_BEGIN = 1000, - MOTOMAN_MSG_BEGIN = 2000 - }; -} -typedef StandardMsgTypes::StandardMsgType StandardMsgType; - -/** - * \brief Enumeration of communications types (supported by all platforms). - */ -namespace CommTypes -{ - enum CommType - { - INVALID = 0, - TOPIC = 1, - SERVICE_REQUEST = 2, - SERVICE_REPLY = 3 - }; -} -typedef CommTypes::CommType CommType; - -/** - * \brief Enumeration of reply types (supported by all platforms). In cases of - * success or failure, the return data should include the relevant return info. - */ -namespace ReplyTypes -{ - enum ReplyType - { - INVALID = 0, - SUCCESS = 1, - FAILURE = 2 - }; -} -typedef ReplyTypes::ReplyType ReplyType; - - - -/** -* \brief This class defines a simple messaging protocol for communicating with an -* industrial robot controller. -* -* The protocol meets the following requirements: -* -* 1. Format should be simple enough that code can be shared between ROS and -* the controller (for those controllers that support C/C++). For those controllers -* that do not support C/C++, the protocol must be simple enough to be decoded with -* the limited capabilities of the typical robot programming language. A corollary -* to this requirement is that the protocol should not be so onerous as to overwhelm -* the limited resources of the robot controller -* -* 2. Format should allow for data streaming (ROS topic like) -* -* 3. Format should allow for data reply (ROS service like) -* -* 4. The protocol is not intended to encapsulate version information It is up to -* individual developers to ensure that code developed for communicating platforms -* does not have any version conflicts (this includes message type identifiers). -* -* Message Structure -* -* - Not considered part of the message -* - int LENGTH (HEADER + DATA) in bytes -* -* -* -* -
-* - int MSG_TYPE identifies type of message (standard (see StandardMsgTypes::StandardMsgType) -* and robot specific values) -* - int COMM_TYPE identified communications type (see CommTypes::CommType) -* - int REPLY CODE (service reply only) reply code (see ReplyTypes::ReplyType) -* -* - -* - ByteArray DATA variable length data determined by message -* type and and communications type. -* -* -* THIS CLASS IS NOT THREAD-SAFE -* -*/ -class SimpleMessage -{ - - -public: - /** - * \brief Constructs an empty message - */ - SimpleMessage(); - - /** - * \brief Destructs a message - */ - ~SimpleMessage(void); - /** - * \brief Initializes a fully populated simple message - * - * \param message type. Globally unique message ID (see StandardMsgType) - * \param communications types (see CommType) - * \param reply code(see ReplyType), only valide if comms type is a reply - * \param data payload for the message - * - * \return true if valid message created - */ - bool init(int msgType, int commType, int replyCode, - industrial::byte_array::ByteArray &data ); - - /** - * \brief Initializes a simple message with an emtpy data payload - * - * \param message type. Globally unique message ID (see StandardMsgType) - * \param communications types (see CommType) - * \param reply code(see ReplyType), only valide if comms type is a reply - * - * \return true if valid message created - */ - bool init(int msgType, int commType, int replyCode); - - /** - * \brief Initializes a simple message from a generic byte array. The byte - * array is assumed to hold a valid message with a HEADER and data payload - * - * \param valid message (as bytes) - * - * \return true if valid message created - */ - bool init(industrial::byte_array::ByteArray & msg); - - /** - * \brief Populates a raw byte array with the message. Any data stored in - * the passed in byte array is deleted - * - * \param byte array to be populated - */ - void toByteArray(industrial::byte_array::ByteArray & msg); - - /** - * \brief Gets size of message header in bytes(fixed) - * - * \return message header size - */ - static unsigned int getHeaderSize() { return SimpleMessage::HEADER_SIZE; }; - - /** - * \brief Gets size of message length member in bytes (fixed) - * - * \return message header size - */ - static unsigned int getLengthSize() { return SimpleMessage::LENGTH_SIZE; }; - - /** - * \brief Gets message type(see StandardMsgType) - * - * \return message type - */ - int getMessageType() {return this->message_type_;}; - - /** - * \brief Gets message type(see CommType) - * - * \return communications type - */ - int getCommType() {return this->comm_type_;}; - - /** - * \brief Gets reply code(see ReplyType) - * - * \return reply code - */ - int getReplyCode() {return this->reply_code_;}; - - /** - * \brief Gets message length (total size, HEADER + data) - * - * \return message length - */ - int getMsgLength() {return this->getHeaderSize() + this->data_.getBufferSize();}; - - /** - * \brief Gets length of message data portion. - * - * \return message data length - */ - int getDataLength() {return this->data_.getBufferSize();}; - - /** - * \brief Returns a reference to the internal data member - * - * \return reference to message data portion. - */ - industrial::byte_array::ByteArray & getData() {return this->data_;}; - - /** - * \brief performs logical checks to ensure that the message is fully - * defined and adheres to the message conventions. - * - * \return true if message valid, false otherwise - */ - bool validateMessage(); - - - -private: - - /** - * \brief Message type(see StandardMsgType) - */ - industrial::shared_types::shared_int message_type_; - - /** - * \brief Communications type(see CommType) - */ - industrial::shared_types::shared_int comm_type_; - - /** - * \brief Reply code(see ReplyType) - */ - industrial::shared_types::shared_int reply_code_; - - /** - * \brief Message data portion - */ - industrial::byte_array::ByteArray data_; - - /** - * \brief Size(in bytes) of message header (fixed) - */ - static const unsigned int HEADER_SIZE = sizeof(industrial::shared_types::shared_int) + - sizeof(industrial::shared_types::shared_int) + - sizeof(industrial::shared_types::shared_int); - - /** - * \brief Size (in bytes) of message length parameter (fixed) - */ - static const unsigned int LENGTH_SIZE = sizeof(industrial::shared_types::shared_int); - - /** - * \brief Sets message type - * - * \param message type - */ - void setMessageType(int msgType) {this->message_type_ = msgType;}; - - /** - * \brief Sets communications type - * - * \param communications type - */ - void setCommType(int commType) {this->comm_type_ = commType;}; - - /** - * \brief Sets reply code - * - * \param reply code - */ - void setReplyCode(int replyCode) {this->reply_code_ = replyCode;}; - - /** - * \brief Sets data portion - * - * \param data portion - */ - void setData(industrial::byte_array::ByteArray & data); -}; - -}//namespace simple_message -}//namespace industrial - -#endif //SIMPLE_MSG_ diff --git a/training/supplements/src/industrial_core/simple_message/include/simple_message/simple_serialize.h b/training/supplements/src/industrial_core/simple_message/include/simple_message/simple_serialize.h deleted file mode 100644 index 366b57d7..00000000 --- a/training/supplements/src/industrial_core/simple_message/include/simple_message/simple_serialize.h +++ /dev/null @@ -1,92 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2011, Southwest Research Institute - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in the - * documentation and/or other materials provided with the distribution. - * * Neither the name of the Southwest Research Institute, nor the names - * of its contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef SIMPLE_SERIALIZE_H -#define SIMPLE_SERIALIZE_H - -#ifndef FLATHEADERS -#include "simple_message/byte_array.h" -#else -#include "byte_array.h" -#endif - -#include "string.h" - -namespace industrial -{ -namespace simple_serialize -{ - - - /** - * \brief Interface for loading and unloading a class to/from a ByteArray - */ -class SimpleSerialize -{ -public: - /** - * \brief Virtual method for loading an object into a ByteArray - * - * This method should load all the required data to reconstruct the class - * object into the buffer - * - * \param buffer pointer to ByteArray - * - * \return true on success, false otherwise (buffer not large enough) - */ - virtual bool load(industrial::byte_array::ByteArray *buffer)=0; - - /** - * \brief Virtual method for unloading an object from a ByteArray - * - * This method should unload all the required data to reconstruct - * the class object (in place) - * - * \param buffer pointer to ByteArray - * - * \return true on success, false otherwise (buffer not large enough) - */ - virtual bool unload(industrial::byte_array::ByteArray *buffer)=0; - - /** - * \brief Virtual method returns the object size when packed into a - * ByteArray - * - * \return object size (in bytes) - */ - virtual unsigned int byteLength()=0; - -}; - -} // namespace simple_serialize -} // namespace industrial - -#endif //SIMPLE_SERIALIZE_H diff --git a/training/supplements/src/industrial_core/simple_message/include/simple_message/smpl_msg_connection.h b/training/supplements/src/industrial_core/simple_message/include/simple_message/smpl_msg_connection.h deleted file mode 100755 index f94a6165..00000000 --- a/training/supplements/src/industrial_core/simple_message/include/simple_message/smpl_msg_connection.h +++ /dev/null @@ -1,144 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2011, Southwest Research Institute - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in the - * documentation and/or other materials provided with the distribution. - * * Neither the name of the Southwest Research Institute, nor the names - * of its contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef SIMPLE_MESSAGE_CONNECTION_H -#define SIMPLE_MESSAGE_CONNECTION_H - -#ifndef FLATHEADERS -#include "simple_message/byte_array.h" -#include "simple_message/simple_message.h" -#include "simple_message/shared_types.h" -#else -#include "byte_array.h" -#include "simple_message.h" -#include "shared_types.h" -#endif - - -namespace industrial -{ -namespace smpl_msg_connection -{ - -/** - * \brief Defines an interface and common methods for sending simple messages - * (see simple_message). This interface makes a bare minimum of assumptions: - * - * 1. The connection is capable of sending raw bytes (encapsulated within a simple message) - * - * 2. The data connection has an explicit connect that establishes the connection (and an - * associated disconnect method). NOTE: For data connections that are connectionless, - * such as UDP, the connection method can be a NULL operation. - */ -class SmplMsgConnection - -{ -public: - - // Message - - /** - * \brief Sends a message using the data connection - * - * \param message to send - * - * \return true if successful - */ - virtual bool sendMsg(industrial::simple_message::SimpleMessage & message); - - /** - * \brief Receives a message using the data connection - * - * \param populated with received message - * - * \return true if successful - */ - virtual bool receiveMsg(industrial::simple_message::SimpleMessage & message); - - /** - * \brief Performs a complete send and receive. This is helpful when sending - * a message that requires and explicit reply - * - * \param message to send - * \param populated with received message - * \param verbosity level of low level logging - * - * \return true if successful - */ - bool sendAndReceiveMsg(industrial::simple_message::SimpleMessage & send, - industrial::simple_message::SimpleMessage & recv, - bool verbose = false); - - /** - * \brief return connection status - * - * \return true if connected - */ - virtual bool isConnected()=0; - - /** - * \brief connects to the remote host - * - * \return true on success, false otherwise - */ - virtual bool makeConnect()=0; - -private: - - // Overrides - /** - * \brief Method used by send message interface method. This should be overridden - * for the specific connection type - * - * \param data to send. - * - * \return true if successful - */ - virtual bool sendBytes(industrial::byte_array::ByteArray & buffer) =0; - - /** - * \brief Method used by receive message interface method. This should be overridden - * for the specific connection type - * - * \param data to receive. - * \param size (in bytes) of data to receive - * - * \return true if successful - */ - virtual bool receiveBytes(industrial::byte_array::ByteArray & buffer, - industrial::shared_types::shared_int num_bytes) =0; - -}; - -} //namespace message_connection -} //namespace industrial - -#endif //SIMPLE_MESSAGE_CONNECTION_H diff --git a/training/supplements/src/industrial_core/simple_message/include/simple_message/socket/simple_socket.h b/training/supplements/src/industrial_core/simple_message/include/simple_message/socket/simple_socket.h deleted file mode 100644 index 6151754d..00000000 --- a/training/supplements/src/industrial_core/simple_message/include/simple_message/socket/simple_socket.h +++ /dev/null @@ -1,256 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2011, Southwest Research Institute - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in the - * documentation and/or other materials provided with the distribution. - * * Neither the name of the Southwest Research Institute, nor the names - * of its contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef SIMPLE_SOCKET_H -#define SIMPLE_SOCKET_H - -#ifndef FLATHEADERS -#include "simple_message/log_wrapper.h" -#include "simple_message/shared_types.h" -#include "simple_message/smpl_msg_connection.h" -#else -#include "log_wrapper.h" -#include "shared_types.h" -#include "smpl_msg_connection.h" -#endif - -#ifdef LINUXSOCKETS - -#include "sys/socket.h" -#include "arpa/inet.h" -#include "string.h" -#include "unistd.h" -#include "netinet/tcp.h" -#include "errno.h" - -#define SOCKET(domain, type, protocol) socket(domain, type, protocol) -#define BIND(sockfd, addr, addrlen) bind(sockfd, addr, addrlen) -#define SET_NO_DELAY(sockfd, val) setsockopt(sockfd, IPPROTO_TCP, TCP_NODELAY, &val, sizeof(val)) -#define SET_REUSE_ADDR(sockfd, val) setsockopt(sockfd, SOL_SOCKET, SO_REUSEADDR, &val, sizeof(val)) -#define LISTEN(sockfd, n) listen(sockfd, n) -#define ACCEPT(sockfd, addr, addrlen) accept(sockfd, addr, addrlen) -#define CONNECT(sockfd, dest_addr ,addrlen) connect(sockfd, dest_addr, addrlen) -#define SEND_TO(sockfd, buf, len, flags, dest_addr, addrlen) sendto(sockfd, buf, len, flags, dest_addr, addrlen) -#define SEND(sockfd, buf, len, flags) send(sockfd, buf, len, flags) -#define RECV_FROM(sockfd, buf, len, flags, src_addr, addrlen) recvfrom(sockfd, buf, len, flags, src_addr, addrlen) -#define RECV(sockfd, buf, len, flags) recv(sockfd, buf, len, flags) -#define SELECT(n, readfds, writefds, exceptfds, timeval) select(n, readfds, writefds, exceptfds, timeval) -#define CLOSE(fd) close(fd) -#ifndef HTONS // OSX defines HTONS -#define HTONS(num) htons(num) -#endif -#define INET_ADDR(str) inet_addr(str) -#define SOCKLEN_T socklen_t - -#endif - -#ifdef MOTOPLUS - -#include "motoPlus.h" - -#include "errno.h" - -// Including os defintion for set socket option. The motoplus wrappers do not give access to socket -// options. In order to remove system delays the nagel algorithm must be disabled using the -// TPC_NO_DELAY option -extern "C" STATUS setsockopt ( /* remove "extern C", if you're using C instead of C++ */ - int s, /* target socket */ - int level, /* protocol level of option */ - int optname, /* option name */ - char * optval, /* pointer to option value */ - int optlen /* option length */ - ); - -#define SOCKET(domain, type, protocol) mpSocket(domain, type, protocol) -#define BIND(sockfd, addr, addrlen) mpBind(sockfd, addr, addrlen) - -// Motoplus compliant version (i.e. a NOOP) -// #define SET_NO_DELAY(sockfd, val) -1 //MOTOPLUS does not allow for setting the "no delay" socket option -// Raw OS call, not Motoplus compliant and might not be allowed in future versions. (taking a risk at this point) -#define SET_NO_DELAY(sockfd, val) setsockopt(sockfd, SOL_SOCKET, TCP_NODELAY, (char *)&val, sizeof(val)) - -#define SET_REUSE_ADDR(sockfd, val) -1 //MOTOPLUS does not support this function. -#define LISTEN(sockfd, n) mpListen(sockfd, n) -#define ACCEPT(sockfd, addr, addrlen) mpAccept(sockfd, addr, addrlen) -#define CONNECT(sockfd, dest_addr ,addrlen) mpConnect(sockfd, dest_addr, addrlen) -#define SEND_TO(sockfd, buf, len, flags, dest_addr, addrlen) mpSendTo(sockfd, buf, len, flags, dest_addr, addrlen) -#define SEND(sockfd, buf, len, flags) mpSend(sockfd, buf, len, flags) -#define RECV_FROM(sockfd, buf, len, flags, src_addr, addrlen) mpRecvFrom(sockfd, buf, len, flags, src_addr, (int*)addrlen) -#define RECV(sockfd, buf, len, flags) mpRecv(sockfd, buf, len, flags) -#define SELECT(n, readfds, writefds, exceptfds, timeval) mpSelect(n, readfds, writefds, exceptfds, timeval) -#define CLOSE(fd) mpClose(fd) -#define HTONS(num) mpHtons(num) -#define INET_ADDR(str) mpInetAddr(str) -#define SOCKLEN_T unsigned int - -#endif - -namespace industrial -{ -namespace simple_socket -{ - -/** - * \brief Enumeration of standard socket ports (supported by all platforms). - * These are defined for convenience. Other ports may be used. Additional - * ports for application specific needs may also be defined. - */ -namespace StandardSocketPorts -{ -enum StandardSocketPort -{ - MOTION = 11000, SYSTEM = 11001, STATE = 11002, IO = 11003 -}; -} -typedef StandardSocketPorts::StandardSocketPort StandardSocketPort; - -/** - * \brief Defines socket functions required for a simple connection type. - */ -class SimpleSocket : public industrial::smpl_msg_connection::SmplMsgConnection -{ -public: - - /** - * \brief Constructor - */ - SimpleSocket(){} - - /** - * \brief Destructor - */ - virtual ~SimpleSocket(){} - - bool isConnected() - { - return connected_; - } - - /** - * \brief returns true if socket data is ready to receive - * - * \param timeout (ms) negative or zero values result in blocking - * - * \return true if data is ready to recieve - */ - bool isReadyReceive(int timeout) - { - bool r, e; - return poll(timeout, r, e); - } - -protected: - - /** - * \brief socket handle for sending/receiving data - */ - int sock_handle_; - - /** - * \brief address/port of remote socket - */ - sockaddr_in sockaddr_; - - /** - * \brief flag indicating socket connection status - */ - bool connected_; - - /** - * \brief socket fail return value - */ - static const int SOCKET_FAIL = -1; - - /** - * \brief maximum size of buffer for receiving data (fixed memory size used - * in order to avoid dynamic memory allocation) - */ - static const int MAX_BUFFER_SIZE = 1024; - - /** - * \brief socket ready polling timeout (ms) - */ - static const int SOCKET_POLL_TO = 1000; - - /** - * \brief internal data buffer for receiving - */ - char buffer_[MAX_BUFFER_SIZE + 1]; - - int getSockHandle() const - { - return sock_handle_; - } - - void setSockHandle(int sock_handle_) - { - this->sock_handle_ = sock_handle_; - } - - virtual void setConnected(bool connected) - { - this->connected_ = connected; - } - - void logSocketError(const char* msg, int rc) - { - int errno_ = errno; - LOG_ERROR("%s, rc: %d. Error: '%s' (errno: %d)", msg, rc, strerror(errno_), errno_); - } - - /** - * \brief polls socket for data or error - * - * \param timeout (ms) negative or zero values result in blocking - * \param ready true if ready - * \param except true if exception - * - * \return true if function DID NOT timeout (must check flags) - */ - bool poll(int timeout, bool & ready, bool & error); - - // Send/Receive functions (inherited classes should override raw methods - // Virtual - bool sendBytes(industrial::byte_array::ByteArray & buffer); - bool receiveBytes(industrial::byte_array::ByteArray & buffer, - industrial::shared_types::shared_int num_bytes); - // Virtual - virtual int rawSendBytes(char *buffer, - industrial::shared_types::shared_int num_bytes)=0; - virtual int rawReceiveBytes(char *buffer, - industrial::shared_types::shared_int num_bytes)=0; - -}; - -} //simple_socket -} //industrial - -#endif /* SIMPLE_SOCKET_H */ diff --git a/training/supplements/src/industrial_core/simple_message/include/simple_message/socket/tcp_client.h b/training/supplements/src/industrial_core/simple_message/include/simple_message/socket/tcp_client.h deleted file mode 100644 index 035128c0..00000000 --- a/training/supplements/src/industrial_core/simple_message/include/simple_message/socket/tcp_client.h +++ /dev/null @@ -1,85 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2011, Southwest Research Institute - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in the - * documentation and/or other materials provided with the distribution. - * * Neither the name of the Southwest Research Institute, nor the names - * of its contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef TCP_CLIENT_H -#define TCP_CLIENT_H - -#ifndef FLATHEADERS -#include "simple_message/socket/tcp_socket.h" -#else -#include "tcp_socket.h" -#endif - - -namespace industrial -{ -namespace tcp_client -{ - - -/** - * \brief Defines TCP client functions. - */ -class TcpClient : public industrial::tcp_socket::TcpSocket -{ -public: - - /** - * \brief Constructor - */ - TcpClient(); - - /** - * \brief Destructor - */ - ~TcpClient(); - - /** - * \brief initializes TCP client socket. Object can either be a client OR - * a server, NOT BOTH. - * - * \param buff server address (in string form) xxx.xxx.xxx.xxx - * \param port_num port number (server & client port number must match) - * - * \return true on success, false otherwise (socket is invalid) - */ - bool init(char *buff, int port_num); - - // Overrides - bool makeConnect(); - - -}; - -} //tcp_client -} //industrial - -#endif /* TCP_CLIENT_H */ diff --git a/training/supplements/src/industrial_core/simple_message/include/simple_message/socket/tcp_server.h b/training/supplements/src/industrial_core/simple_message/include/simple_message/socket/tcp_server.h deleted file mode 100644 index 357bb1e9..00000000 --- a/training/supplements/src/industrial_core/simple_message/include/simple_message/socket/tcp_server.h +++ /dev/null @@ -1,98 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2011, Southwest Research Institute - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in the - * documentation and/or other materials provided with the distribution. - * * Neither the name of the Southwest Research Institute, nor the names - * of its contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef TCP_SERVER_H -#define TCP_SERVER_H - -#ifndef FLATHEADERS -#include "simple_message/socket/tcp_socket.h" -#else -#include "tcp_socket.h" -#endif - -namespace industrial -{ -namespace tcp_server -{ - -/** - * \brief Defines TCP server functions. - */ -class TcpServer : public industrial::tcp_socket::TcpSocket -{ -public: - - /** - * \brief Constructor - */ - TcpServer(); - - /** - * \brief Destructor - */ - ~TcpServer(); - - /** - * \brief initializes TCP server socket. The connect method must be called - * following initialization in order to communicate with the remote host. - * - * \param port_num port number (server & client port number must match) - * - * \return true on success, false otherwise (socket is invalid) - */ - bool init(int port_num); - - // Overrides - bool makeConnect(); - -protected: - /** - * \brief server handle. Every time a connection is made, the class generates - * a new handle for sending/receiving. The server handle is saved off to a - * separate variable so that recoving a lost connection is possible. - */ - int srvr_handle_; - - int getSrvrHandle() const - { - return srvr_handle_; - } - - void setSrvrHandle(int srvr_handle_) - { - this->srvr_handle_ = srvr_handle_; - } -}; - -} //simple_socket -} //industrial - -#endif /* TCP_SERVER_H */ diff --git a/training/supplements/src/industrial_core/simple_message/include/simple_message/socket/tcp_socket.h b/training/supplements/src/industrial_core/simple_message/include/simple_message/socket/tcp_socket.h deleted file mode 100644 index 43245b21..00000000 --- a/training/supplements/src/industrial_core/simple_message/include/simple_message/socket/tcp_socket.h +++ /dev/null @@ -1,80 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2011, Southwest Research Institute - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in the - * documentation and/or other materials provided with the distribution. - * * Neither the name of the Southwest Research Institute, nor the names - * of its contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef TCP_SOCKET_H -#define TCP_SOCKET_H - -#ifndef FLATHEADERS -#include "simple_message/socket/simple_socket.h" -#include "simple_message/shared_types.h" -#else -#include "simple_socket.h" -#include "shared_types.h" -#endif - -#ifdef LINUXSOCKETS -#include "sys/socket.h" -#include "arpa/inet.h" -#include "string.h" -#include "unistd.h" -#endif - -#ifdef MOTOPLUS -#include "motoPlus.h" -#endif - - -namespace industrial -{ -namespace tcp_socket -{ - -class TcpSocket : public industrial::simple_socket::SimpleSocket -{ -public: - - TcpSocket(); - virtual ~TcpSocket(); - -private: - - // Virtual - int rawSendBytes(char *buffer, - industrial::shared_types::shared_int num_bytes); - int rawReceiveBytes(char *buffer, - industrial::shared_types::shared_int num_bytes); - -}; - -} //tcp_socket -} //industrial - -#endif /* TCP_SOCKET_H */ diff --git a/training/supplements/src/industrial_core/simple_message/include/simple_message/socket/udp_client.h b/training/supplements/src/industrial_core/simple_message/include/simple_message/socket/udp_client.h deleted file mode 100644 index 8d360aec..00000000 --- a/training/supplements/src/industrial_core/simple_message/include/simple_message/socket/udp_client.h +++ /dev/null @@ -1,75 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2011, Yaskawa America, Inc. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in the - * documentation and/or other materials provided with the distribution. - * * Neither the name of the Yaskawa America, Inc., nor the names - * of its contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef UDP_CLIENT_H -#define UDP_CLIENT_H - -#ifndef FLATHEADERS -#include "simple_message/socket/udp_socket.h" -#else -#include "udp_socket.h" -#endif - - -namespace industrial -{ -namespace udp_client -{ - -class UdpClient : public industrial::udp_socket::UdpSocket -{ -public: - - UdpClient(); - ~UdpClient(); - - bool makeConnect(); -/* - * \brief initializes UDP client socket - * - * \param buff server address (in string form) xxx.xxx.xxx.xxx - * \param port_num port number (server & client port number must match) - * - * \return true on success, false otherwise (socket is invalid) - */ - bool init(char *buff, int port_num); - - -private: - - -}; - -} //udp_server -} //industrial - -#endif - diff --git a/training/supplements/src/industrial_core/simple_message/include/simple_message/socket/udp_server.h b/training/supplements/src/industrial_core/simple_message/include/simple_message/socket/udp_server.h deleted file mode 100644 index 04dc1e98..00000000 --- a/training/supplements/src/industrial_core/simple_message/include/simple_message/socket/udp_server.h +++ /dev/null @@ -1,74 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2011, Yaskawa America, Inc. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in the - * documentation and/or other materials provided with the distribution. - * * Neither the name of the Yaskawa America, Inc., nor the names - * of its contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef UDP_SERVER_H -#define UDP_SERVER_H - -#ifndef FLATHEADERS -#include "simple_message/socket/udp_socket.h" -#else -#include "udp_socket.h" -#endif - - -namespace industrial -{ -namespace udp_server -{ - -class UdpServer : public industrial::udp_socket::UdpSocket -{ -public: - - UdpServer(); - ~UdpServer(); - - bool makeConnect(); - /** - * \brief initializes UDP server socket. - * - * \param port_num port number (server & client port number must match) - * - * \return true on success, false otherwise (socket is invalid) - */ - bool init(int port_num); - - -private: - - -}; - -} //udp_server -} //industrial - -#endif - diff --git a/training/supplements/src/industrial_core/simple_message/include/simple_message/socket/udp_socket.h b/training/supplements/src/industrial_core/simple_message/include/simple_message/socket/udp_socket.h deleted file mode 100644 index 5846ee5a..00000000 --- a/training/supplements/src/industrial_core/simple_message/include/simple_message/socket/udp_socket.h +++ /dev/null @@ -1,95 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2011, Yaskawa America, Inc. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in the - * documentation and/or other materials provided with the distribution. - * * Neither the name of the Yaskawa America, Inc., nor the names - * of its contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef UDP_SOCKET_H -#define UDP_SOCKET_H - -#ifndef FLATHEADERS -#include "simple_message/socket/simple_socket.h" -#include "simple_message/shared_types.h" -#include "simple_message/smpl_msg_connection.h" -#else -#include "simple_socket.h" -#include "shared_types.h" -#include "smpl_msg_connection.h" -#endif - -#ifdef LINUXSOCKETS -#include "sys/socket.h" -#include "arpa/inet.h" -#include "string.h" -#include "unistd.h" -#endif - -#ifdef MOTOPLUS -#include "motoPlus.h" -#endif - -namespace industrial -{ -namespace udp_socket -{ - -class UdpSocket : public industrial::simple_socket::SimpleSocket -{ -public: - - UdpSocket(); - ~UdpSocket(); - - // Override - // receive is overridden because the base class implementation assumed - // socket data could be read partially. UDP socket data is lost when - // only a portion of it is read. For that reason this receive method - // reads the entire data stream (assumed to be a single message). - bool receiveMsg(industrial::simple_message::SimpleMessage & message); - - -protected: - - /** - * \brief udp socket connect handshake value - */ - static const char CONNECT_HANDSHAKE = 255; - - // Virtual - int rawSendBytes(char *buffer, - industrial::shared_types::shared_int num_bytes); - int rawReceiveBytes(char *buffer, - industrial::shared_types::shared_int num_bytes); - -}; - -} //udp_socket -} //industrial - -#endif - diff --git a/training/supplements/src/industrial_core/simple_message/include/simple_message/typed_message.h b/training/supplements/src/industrial_core/simple_message/include/simple_message/typed_message.h deleted file mode 100644 index a2299d84..00000000 --- a/training/supplements/src/industrial_core/simple_message/include/simple_message/typed_message.h +++ /dev/null @@ -1,167 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2011, Southwest Research Institute - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in the - * documentation and/or other materials provided with the distribution. - * * Neither the name of the Southwest Research Institute, nor the names - * of its contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef TYPED_MESSAGE_H -#define TYPED_MESSAGE_H - -#ifndef FLATHEADERS -#include "simple_message/simple_message.h" -#include "simple_message/byte_array.h" -#else -#include "simple_message.h" -#include "byte_array.h" -#endif - - -namespace industrial -{ -namespace typed_message -{ - -/** - * \brief Message interface for typed messages built from SimpleMessage. - * - * This is an interface for a helper class that when implemented is used - * to create simple messages of the various types (i.e. as defined by the - * message type enumeration). It also has constructors and initializers - * that can be used to create a typed message from a simple message. - * - * If the typed message does not support a particular simple message type - * the "to" method should be overridden to return false. For exmaple, a - * ping message cannot be a topic, it is always expected to be a request/ - * reply. A joint trajectory point on the other hand may either be a topic - * (i.e. asynchronously sent) or a request/reply (i.e. syncrounously sent) - * - * Classes that implement this interface shall include data members for - * the data payload of the typed message. - * - * \deprecated The base function implementations in the class will be removed - * in a later release. This will force classes that inherit from this - * class to implement them. - * - * THIS CLASS IS NOT THREAD-SAFE - * - */ - -class TypedMessage : public industrial::simple_serialize::SimpleSerialize -{ - -public: - /** - * \brief Initializes message from a simple message - * - * \return true if message successfully initialized, otherwise false - */ - virtual bool init(industrial::simple_message::SimpleMessage & msg)=0; - - /** - * \brief Initializes a new empty message - * - */ - virtual void init()=0; - - /** - * \brief creates a simple_message request - * - * \return true if message successfully initialized, otherwise false - */ - virtual bool toRequest(industrial::simple_message::SimpleMessage & msg) - { - industrial::byte_array::ByteArray data; - data.load(*this); - return msg.init(this->getMessageType(), - industrial::simple_message::CommTypes::SERVICE_REQUEST, - industrial::simple_message::ReplyTypes::INVALID, data); - } - - /** - * \brief creates a simple_message reply - * - * \return true if message successfully initialized, otherwise false - */ - virtual bool toReply(industrial::simple_message::SimpleMessage & msg, - industrial::simple_message::ReplyType reply) - { - industrial::byte_array::ByteArray data; - data.load(*this); - return msg.init(this->getMessageType(), - industrial::simple_message::CommTypes::SERVICE_REPLY, - reply, data); - } - /** - * \brief creates a simple_message topic - * - * \return true if message successfully initialized, otherwise false - */ - virtual bool toTopic(industrial::simple_message::SimpleMessage & msg) - { - industrial::byte_array::ByteArray data; - data.load(*this); - return msg.init(this->getMessageType(), - industrial::simple_message::CommTypes::TOPIC, - industrial::simple_message::ReplyTypes::INVALID, data); - } - /** - * \brief gets message type (enumeration) - * - * \return message type - */ - int getMessageType() const - { - return message_type_; - } - -protected: - -/** - * \brief sets message type - * - * \return message type - */ - void setMessageType(int message_type = industrial::simple_message::StandardMsgTypes::INVALID) - { - this->message_type_ = message_type; - } - -private: - - /** - * \brief Message type expected by callback - */ - - int message_type_; - -}; - -} -} - -#endif /* TYPED_MESSAGE_H */ diff --git a/training/supplements/src/industrial_core/simple_message/mainpage.dox b/training/supplements/src/industrial_core/simple_message/mainpage.dox deleted file mode 100644 index 797c3e33..00000000 --- a/training/supplements/src/industrial_core/simple_message/mainpage.dox +++ /dev/null @@ -1,20 +0,0 @@ -/** -\mainpage -\htmlinclude manifest.html - -\b simple_message defines a simple messaging connection and protocol for communicating with an industrial robot controller. Additional handler and manager classes are included for handling connection limited systems. - - - - -\section codeapi Code API - -The main APIs of are largely captured in the following interface classes: -- industrial::simple_message::SimpleMessage : Defines the protocol for messaging between ROS nodes and a robot controller. -- industrial::smpl_msg_connection::SmplMsgConnection : Defines the connection interface for sending simple messages. -- industrial::message_handler::MessageHandler : Defines a callback interface for handling message of a specific type. -- industrial::message_manager::MessageManager : Manages multiple message handlers on a single connection (useful for connection limited systems, such as robot controllers). - -*/ diff --git a/training/supplements/src/industrial_core/simple_message/package.xml b/training/supplements/src/industrial_core/simple_message/package.xml deleted file mode 100644 index 7f9f2580..00000000 --- a/training/supplements/src/industrial_core/simple_message/package.xml +++ /dev/null @@ -1,20 +0,0 @@ - - simple_message - 0.3.3 - - simple_message defines a simple messaging connection and protocol for communicating - with an industrial robot controller. Additional handler and manager classes are - included for handling connection limited systems. This package is part of the ROS-Industrial - program. - - Shaun Edwards - BSD - http://ros.org/wiki/simple_message - Shaun Edwards - - catkin - roscpp - industrial_msgs - roscpp - industrial_msgs - diff --git a/training/supplements/src/industrial_core/simple_message/src/byte_array.cpp b/training/supplements/src/industrial_core/simple_message/src/byte_array.cpp deleted file mode 100644 index a79ada6f..00000000 --- a/training/supplements/src/industrial_core/simple_message/src/byte_array.cpp +++ /dev/null @@ -1,502 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2011, Southwest Research Institute - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in the - * documentation and/or other materials provided with the distribution. - * * Neither the name of the Southwest Research Institute, nor the names - * of its contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ -#ifndef FLATHEADERS -#include "simple_message/byte_array.h" -#include "simple_message/simple_serialize.h" -#include "simple_message/log_wrapper.h" -#else -#include "byte_array.h" -#include "simple_serialize.h" -#include "log_wrapper.h" -#endif - -#ifdef MOTOPLUS -#include "motoPlus.h" -#endif - -#include "string.h" - -namespace industrial -{ -namespace byte_array -{ - -using namespace industrial::simple_serialize; -using namespace industrial::shared_types; -using namespace industrial::byte_array; - -ByteArray::ByteArray(void) -{ - this->init(); -#ifdef BYTE_SWAPPING - LOG_COMM("Byte swapping enabled"); -#endif -} - -ByteArray::~ByteArray(void) -{ -} - -void ByteArray::init() -{ - memset(&(buffer_[0]), 0, this->MAX_SIZE); - this->setBufferSize(0); -} - -bool ByteArray::init(const char* buffer, const shared_int byte_size) -{ - bool rtn; - - if (this->MAX_SIZE >= byte_size) - { - LOG_COMM("Initializing buffer to size: %d", byte_size); - this->load((void*)buffer, byte_size); - rtn = true; - } - else - { - LOG_ERROR("Failed to initialize byte array, buffer size: %u greater than max: %u", - byte_size, this->getMaxBufferSize()); - rtn = false; - } - return rtn; -} - -void ByteArray::copyFrom(ByteArray & buffer) -{ - if (buffer.getBufferSize() != 0) - { - this->setBufferSize(buffer.getBufferSize()); - memcpy(this->getRawDataPtr(), buffer.getRawDataPtr(), this->buffer_size_); - } - else - { - LOG_WARN("Byte array copy not performed, buffer to copy is empty"); - } -} - - -#ifdef BYTE_SWAPPING -void ByteArray::swap(void *value, shared_int byteSize) -{ - LOG_COMM("Executing byte swapping"); - - LOG_COMM("Value (swapping-input): %u", (unsigned int)(*(unsigned int*)value)); - for (unsigned int i = 0; i < byteSize / 2; i++) - { - unsigned int endIndex = byteSize - i - 1; - char endByte = ((char*)value)[endIndex]; - unsigned int endInt = endByte; - - unsigned int beginIndex = i; - char beginByte = ((char*)value)[beginIndex]; - unsigned int beginInt = beginByte; - - LOG_COMM("Swap beginIndex i: %u, endIndex: %u, begin[]: %u, end[]: %u", - beginIndex, endIndex, beginInt, endInt); - ((char*)value)[endIndex] = beginByte; - ((char*)value)[beginIndex] = endByte; - } - LOG_COMM("Value (swapping-output): %u", (unsigned int)(*(unsigned int*)value)); - -} -#endif - -char* ByteArray::getRawDataPtr() -{ - return &this->buffer_[0]; -} - -/**************************************************************** - // load(*) - // - // Methods for loading various data types. - // - */ -bool ByteArray::load(shared_bool value) -{ - return this->load(&value, sizeof(shared_bool)); -} - -bool ByteArray::load(shared_real value) -{ -#ifdef BYTE_SWAPPING - LOG_COMM("Value (loading-input): %f", value); - this->swap(&value, sizeof(shared_real)); - LOG_COMM("Value (loading-output): %f", value); -#endif - - return this->load(&value, sizeof(shared_real)); -} - -bool ByteArray::load(shared_int value) -{ -#ifdef BYTE_SWAPPING - LOG_COMM("Value (loading-input): %d", value); - this->swap(&value, sizeof(shared_int)); - LOG_COMM("Value (loading-output): %d", value); -#endif - - return this->load(&value, sizeof(shared_int)); -} - -bool ByteArray::load(simple_serialize::SimpleSerialize &value) -{ - LOG_COMM("Executing byte array load through simple serialize"); - return value.load(this); -} - -bool ByteArray::load(ByteArray &value) -{ - LOG_COMM("Executing byte array load through byte array"); - return this->load(value.getRawDataPtr(), value.getBufferSize()); -} - -bool ByteArray::load(void* value, const shared_int byte_size) -{ - - bool rtn; - // Get the load pointer before extending the buffer. - char* loadPtr; - - LOG_COMM("Executing byte array load through void*, size: %d", byte_size); - // Check inputs - if (NULL == value) - { - LOG_ERROR("NULL point passed into load method"); - return false; - } - - loadPtr = this->getLoadPtr(); - - if (this->extendBufferSize(byte_size)) - { - memcpy(loadPtr, value, byte_size); - rtn = true; - } - else - { - LOG_ERROR("Failed to load byte array"); - rtn = false; - } - - return rtn; -} - -/**************************************************************** - // unload(*) - // - // Methods for unloading various data types. Unloading data shortens - // the internal buffer. The resulting memory that holds the data is - // lost. - // - */ -bool ByteArray::unload(shared_bool & value) -{ - shared_bool rtn = this->unload(&value, sizeof(shared_bool)); - return rtn; - -} - -bool ByteArray::unload(shared_real &value) -{ - bool rtn = this->unload(&value, sizeof(shared_real)); - -#ifdef BYTE_SWAPPING - LOG_COMM("Value (unloading-input): %f", value); - this->swap(&value, sizeof(shared_real)); - LOG_COMM("Value (unloading-output): %f", value); -#endif - - return rtn; -} - -bool ByteArray::unload(shared_int &value) -{ - bool rtn = this->unload(&value, sizeof(shared_int)); - -#ifdef BYTE_SWAPPING - LOG_COMM("Value (unloading-input): %d", value); - this->swap(&value, sizeof(shared_int)); - LOG_COMM("Value (unloading-output): %d", value); -#endif - return rtn; -} - -bool ByteArray::unload(simple_serialize::SimpleSerialize &value) -{ - LOG_COMM("Executing byte array unload through simple serialize"); - return value.unload(this); -} - -bool ByteArray::unload(ByteArray &value, const shared_int byte_size) -{ - LOG_COMM("Executing byte array unload through byte array"); - char* unloadPtr = this->getUnloadPtr(byte_size); - bool rtn; - - if (NULL != unloadPtr) - { - if (this->shortenBufferSize(byte_size)) - { - - rtn = value.load(unloadPtr, byte_size); - rtn = true; - } - else - { - LOG_ERROR("Failed to shorten array"); - rtn = false; - } - } - else - { - LOG_ERROR("Unload pointer returned NULL"); - rtn = false; - } - - return rtn; -} - -bool ByteArray::unload(void* value, shared_int byteSize) -{ - bool rtn; - char* unloadPtr; - - LOG_COMM("Executing byte array unload through void*, size: %d", byteSize); - // Check inputs - if (NULL == value) - { - LOG_ERROR("NULL point passed into unload method"); - return false; - } - - unloadPtr = this->getUnloadPtr(byteSize); - - if (NULL != unloadPtr) - { - - if (this->shortenBufferSize(byteSize)) - { - memcpy(value, unloadPtr, byteSize); - rtn = true; - } - else - { - LOG_ERROR("Failed to shorten array"); - rtn = false; - } - } - else - { - LOG_ERROR("Unload pointer returned NULL"); - rtn = false; - } - - return rtn; -} - - - -/**************************************************************** - // unloadFront(*) - // - // Methods for unloading various data types. Unloading data shortens - // the internal buffer and requires a memmove. These functions should - // be used sparingly, as they are expensive. - // - */ -bool ByteArray::unloadFront(industrial::shared_types::shared_real &value) -{ - bool rtn = this->unloadFront(&value, sizeof(shared_real)); - -#ifdef BYTE_SWAPPING - LOG_COMM("Value (unloading-input): %f", value); - this->swap(&value, sizeof(shared_real)); - LOG_COMM("Value (unloading-output): %f", value); -#endif - return rtn; -} - -bool ByteArray::unloadFront(industrial::shared_types::shared_int &value) -{ - bool rtn = this->unloadFront(&value, sizeof(shared_int)); - -#ifdef BYTE_SWAPPING - LOG_COMM("Value (unloading-input): %d", value); - this->swap(&value, sizeof(shared_int)); - LOG_COMM("Value (unloading-output): %d", value); -#endif - return rtn; -} -bool ByteArray::unloadFront(void* value, const industrial::shared_types::shared_int byteSize) -{ - bool rtn; - char* unloadPtr = NULL; - char* nextPtr = NULL; - shared_int sizeRemain; - - // Check inputs - if (NULL == value) - { - LOG_ERROR("NULL point passed into unload method"); - return false; - } - - unloadPtr = &this->buffer_[0]; - - if (NULL != unloadPtr) - { - nextPtr = unloadPtr + byteSize; - sizeRemain = this->getBufferSize() - byteSize; - - LOG_DEBUG("Unloading: %d bytes, %d bytes remain", byteSize, sizeRemain); - if (this->shortenBufferSize(byteSize)) - { - LOG_COMM("Preparing to copy value"); - memcpy(value, unloadPtr, byteSize); - LOG_COMM("Value is unloaded, performing move"); - memmove(unloadPtr, nextPtr, sizeRemain); - LOG_COMM("Move operation completed"); - rtn = true; - } - else - { - LOG_ERROR("Failed to shorten array"); - rtn = false; - } - } - else - { - LOG_ERROR("Unload pointer returned NULL"); - rtn = false; - } - - return rtn; -} - -unsigned int ByteArray::getBufferSize() -{ - return this->buffer_size_; -} - -unsigned int ByteArray::getMaxBufferSize() -{ - return this->MAX_SIZE; -} - - -bool ByteArray::isByteSwapEnabled() -{ -#ifdef BYTE_SWAPPING - return true; -#endif - return false; -} - -bool ByteArray::setBufferSize(shared_int size) -{ - bool rtn; - - if (this->MAX_SIZE >= size) - { - this->buffer_size_ = size; - rtn = true; - } - else - { - LOG_ERROR("Set buffer size: %u, larger than MAX:, %u", size, this->MAX_SIZE); - rtn = false; - } - - return rtn; - -} - -bool ByteArray::extendBufferSize(shared_int size) -{ - unsigned int newSize; - - newSize = this->getBufferSize() + size; - return this->setBufferSize(newSize); - -} - -bool ByteArray::shortenBufferSize(shared_int size) -{ - unsigned int newSize; - bool rtn; - - // If the buffer is not larger than the size it is shortened by - // we fail. This is checked here (as opposed to setBufferSize) - // because setBufferSize assumes a unsigned argument and therefore - // wouldn't catch a negative size. - if (size <= (shared_int)this->getBufferSize()) - { - newSize = this->getBufferSize() - size; - rtn = this->setBufferSize(newSize); - } - else - { - LOG_ERROR("Failed to shorten buffer by %u bytes, buffer too small, %u bytes", size, this->getBufferSize()); - rtn = false; - } - - return rtn; - -} - -char* ByteArray::getLoadPtr() -{ - - return &this->buffer_[this->buffer_size_]; -} - -char* ByteArray::getUnloadPtr(shared_int byteSize) -{ - char* rtn; - - if (byteSize <= (shared_int)this->getBufferSize()) - { - rtn = this->getLoadPtr() - byteSize; - } - else - { - LOG_ERROR("Get unload pointer failed, buffer size: %d, smaller than byte size: %d", - this->getBufferSize(), byteSize); - rtn = NULL; - } - - return rtn; -} - -} // namespace byte_array -} // namespace industrial diff --git a/training/supplements/src/industrial_core/simple_message/src/joint_data.cpp b/training/supplements/src/industrial_core/simple_message/src/joint_data.cpp deleted file mode 100644 index 87108a9f..00000000 --- a/training/supplements/src/industrial_core/simple_message/src/joint_data.cpp +++ /dev/null @@ -1,178 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2011, Southwest Research Institute - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in the - * documentation and/or other materials provided with the distribution. - * * Neither the name of the Southwest Research Institute, nor the names - * of its contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ -#ifndef FLATHEADERS -#include "simple_message/joint_data.h" -#include "simple_message/shared_types.h" -#include "simple_message/log_wrapper.h" -#else -#include "joint_data.h" -#include "shared_types.h" -#include "log_wrapper.h" -#endif - -using namespace industrial::shared_types; - -namespace industrial -{ -namespace joint_data -{ - -JointData::JointData(void) -{ - this->init(); -} -JointData::~JointData(void) -{ - -} - -void JointData::init() -{ - for (int i = 0; i < this->getMaxNumJoints(); i++) - { - this->setJoint(i, 0.0); - } -} - -bool JointData::setJoint(shared_int index, shared_real value) -{ - bool rtn = false; - - if (index < this->getMaxNumJoints()) - { - this->joints_[index] = value; - rtn = true; - } - else - { - LOG_ERROR("Joint index: %d, is greater than size: %d", index, this->getMaxNumJoints()); - rtn = false; - } - return rtn; -} - -bool JointData::getJoint(shared_int index, shared_real & value) const -{ - bool rtn = false; - - if (index < this->getMaxNumJoints()) - { - value = this->joints_[index]; - rtn = true; - } - else - { - LOG_ERROR("Joint index: %d, is greater than size: %d", index, this->getMaxNumJoints()); - rtn = false; - } - return rtn; -} - -shared_real JointData::getJoint(shared_int index) const -{ - shared_real rtn = 0.0; - this->getJoint(index, rtn); - return rtn; -} - - -void JointData::copyFrom(JointData &src) -{ - shared_real value = 0.0; - - for (int i = 0; i < this->getMaxNumJoints(); i++) - { - src.getJoint(i, value); - this->setJoint(i, value); - } -} - -bool JointData::operator==(JointData &rhs) -{ - bool rtn = true; - - shared_real lhsvalue, rhsvalue; - - for (int i = 0; i < this->getMaxNumJoints(); i++) - { - this->getJoint(i, lhsvalue); - rhs.getJoint(i, rhsvalue); - if (lhsvalue != rhsvalue) - { - rtn = false; - break; - } - } - return rtn; - -} - -bool JointData::load(industrial::byte_array::ByteArray *buffer) -{ - bool rtn = false; - shared_real value = 0.0; - - LOG_COMM("Executing joint position load"); - for (int i = 0; i < this->getMaxNumJoints(); i++) - { - this->getJoint(i, value); - rtn = buffer->load(value); - if (!rtn) - { - LOG_ERROR("Failed to load joint position data"); - break; - } - } - return rtn; -} - -bool JointData::unload(industrial::byte_array::ByteArray *buffer) -{ - bool rtn = false; - shared_real value = 0.0; - - LOG_COMM("Executing joint position unload"); - for (int i = this->getMaxNumJoints() - 1; i >= 0; i--) - { - rtn = buffer->unload(value); - if (!rtn) - { - LOG_ERROR("Failed to unload message joint: %d from data[%d]", i, buffer->getBufferSize()); - break; - } - this->setJoint(i, value); - } - return rtn; -} - -} -} - diff --git a/training/supplements/src/industrial_core/simple_message/src/joint_feedback.cpp b/training/supplements/src/industrial_core/simple_message/src/joint_feedback.cpp deleted file mode 100644 index 98b9eb7a..00000000 --- a/training/supplements/src/industrial_core/simple_message/src/joint_feedback.cpp +++ /dev/null @@ -1,193 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2013, Southwest Research Institute - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in the - * documentation and/or other materials provided with the distribution. - * * Neither the name of the Southwest Research Institute, nor the names - * of its contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ -#ifndef FLATHEADERS -#include "simple_message/joint_feedback.h" -#include "simple_message/shared_types.h" -#include "simple_message/log_wrapper.h" -#else -#include "joint_feedback.h" -#include "shared_types.h" -#include "log_wrapper.h" -#endif - -using namespace industrial::joint_data; -using namespace industrial::shared_types; - -namespace industrial -{ -namespace joint_feedback -{ - -JointFeedback::JointFeedback(void) -{ - this->init(); -} -JointFeedback::~JointFeedback(void) -{ - -} - -void JointFeedback::init() -{ - this->robot_id_ = 0; - this->valid_fields_ = 0; - this->time_ = 0.0; - this->positions_.init(); - this->velocities_.init(); - this->accelerations_.init(); -} - -void JointFeedback::init(industrial::shared_types::shared_int robot_id, - industrial::shared_types::shared_int valid_fields, - industrial::shared_types::shared_real time, - industrial::joint_data::JointData & positions, - industrial::joint_data::JointData & velocities, - industrial::joint_data::JointData & accelerations) -{ - this->setRobotID(robot_id); - this->setTime(time); - this->setPositions(positions); - this->setVelocities(velocities); - this->setAccelerations(accelerations); - this->valid_fields_ = valid_fields; // must happen after others are set -} - -void JointFeedback::copyFrom(JointFeedback &src) -{ - this->setRobotID(src.getRobotID()); - src.getTime(this->time_); - src.getPositions(this->positions_); - src.getVelocities(this->velocities_); - src.getAccelerations(this->accelerations_); - this->valid_fields_ = src.valid_fields_; -} - -bool JointFeedback::operator==(JointFeedback &rhs) -{ - return this->robot_id_ == rhs.robot_id_ && - this->valid_fields_ == rhs.valid_fields_ && - ( !is_valid(ValidFieldTypes::TIME) || (this->time_ == rhs.time_) ) && - ( !is_valid(ValidFieldTypes::POSITION) || (this->positions_ == rhs.positions_) ) && - ( !is_valid(ValidFieldTypes::VELOCITY) || (this->velocities_ == rhs.velocities_) ) && - ( !is_valid(ValidFieldTypes::ACCELERATION) || (this->accelerations_ == rhs.accelerations_) ); -} - -bool JointFeedback::load(industrial::byte_array::ByteArray *buffer) -{ - LOG_COMM("Executing joint feedback load"); - - if (!buffer->load(this->robot_id_)) - { - LOG_ERROR("Failed to load joint feedback robot_id"); - return false; - } - - if (!buffer->load(this->valid_fields_)) - { - LOG_ERROR("Failed to load joint feedback valid fields"); - return false; - } - - if (!buffer->load(this->time_)) - { - LOG_ERROR("Failed to load joint feedback time"); - return false; - } - - if (!this->positions_.load(buffer)) - { - LOG_ERROR("Failed to load joint feedback positions"); - return false; - } - - if (!this->velocities_.load(buffer)) - { - LOG_ERROR("Failed to load joint feedback velocities"); - return false; - } - - if (!this->accelerations_.load(buffer)) - { - LOG_ERROR("Failed to load joint feedback accelerations"); - return false; - } - - LOG_COMM("Joint feedback successfully loaded"); - return true; -} - -bool JointFeedback::unload(industrial::byte_array::ByteArray *buffer) -{ - LOG_COMM("Executing joint feedback unload"); - - if (!this->accelerations_.unload(buffer)) - { - LOG_ERROR("Failed to unload joint feedback accelerations"); - return false; - } - - if (!this->velocities_.unload(buffer)) - { - LOG_ERROR("Failed to unload joint feedback velocities"); - return false; - } - - if (!this->positions_.unload(buffer)) - { - LOG_ERROR("Failed to unload joint feedback positions"); - return false; - } - - if (!buffer->unload(this->time_)) - { - LOG_ERROR("Failed to unload joint feedback time"); - return false; - } - - if (!buffer->unload(this->valid_fields_)) - { - LOG_ERROR("Failed to unload joint feedback valid fields"); - return false; - } - - if (!buffer->unload(this->robot_id_)) - { - LOG_ERROR("Faild to unload joint feedback robot_id"); - return false; - } - - LOG_COMM("Joint feedback successfully unloaded"); - return true; -} - -} -} - diff --git a/training/supplements/src/industrial_core/simple_message/src/joint_traj.cpp b/training/supplements/src/industrial_core/simple_message/src/joint_traj.cpp deleted file mode 100644 index 7754c079..00000000 --- a/training/supplements/src/industrial_core/simple_message/src/joint_traj.cpp +++ /dev/null @@ -1,206 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2011, Southwest Research Institute - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in the - * documentation and/or other materials provided with the distribution. - * * Neither the name of the Southwest Research Institute, nor the names - * of its contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ -#ifndef FLATHEADERS -#include "simple_message/joint_traj.h" -#include "simple_message/shared_types.h" -#include "simple_message/log_wrapper.h" -#else -#include "joint_traj.h" -#include "shared_types.h" -#include "log_wrapper.h" -#endif - -using namespace industrial::shared_types; -using namespace industrial::joint_traj_pt; - -namespace industrial -{ -namespace joint_traj -{ - -JointTraj::JointTraj(void) -{ - this->init(); -} -JointTraj::~JointTraj(void) -{ - -} - -void JointTraj::init() -{ - JointTrajPt empty; - - this->size_ = 0; - for (shared_int i = 0; i < this->getMaxNumPoints(); i++) - { - this->points_[i].copyFrom(empty); - } -} - -bool JointTraj::addPoint(JointTrajPt & point) -{ - bool rtn = false; - - if (!this->isFull()) - { - this->points_[this->size()].copyFrom(point); - this->size_++; - rtn = true; - } - else - { - rtn = false; - LOG_ERROR("Failed to add point, buffer is full"); - } - - return rtn; -} - -bool JointTraj::getPoint(shared_int index, JointTrajPt & point) -{ - bool rtn = false; - - if (index < this->size()) - { - point.copyFrom(this->points_[index]); - rtn = true; - } - else - { - LOG_ERROR("Point index: %d, is greater than size: %d", index, this->size()); - rtn = false; - } - return rtn; -} - -void JointTraj::copyFrom(JointTraj &src) -{ - JointTrajPt value; - - this->size_ = src.size(); - for (shared_int i = 0; i < this->size(); i++) - { - src.getPoint(i, value); - this->points_[i].copyFrom(value); - } -} - -bool JointTraj::operator==(JointTraj &rhs) -{ - bool rtn = true; - - if(this->size() == rhs.size()) - { - for(shared_int i = 0; i < this->size(); i++) - { - JointTrajPt value; - rhs.getPoint(i, value); - if(!(this->points_[i] == value)) - { - LOG_DEBUG("Joint trajectory point different"); - rtn = false; - break; - } - else - { - rtn = true; - } - } - } - else - { - LOG_DEBUG("Joint trajectory compare failed, size mismatch"); - rtn = false; - } - - return rtn; -} - - -bool JointTraj::load(industrial::byte_array::ByteArray *buffer) -{ - bool rtn = false; - JointTrajPt value; - - LOG_COMM("Executing joint trajectory load"); - for (shared_int i = 0; i < this->size(); i++) - { - this->getPoint(i, value); - rtn = buffer->load(value); - if (!rtn) - { - LOG_ERROR("Failed to load joint traj.pt. data"); - rtn = false; - break; - } - rtn = true; - } - - if (rtn) - { - rtn = buffer->load(this->size()); - } - return rtn; -} - -bool JointTraj::unload(industrial::byte_array::ByteArray *buffer) -{ - bool rtn = false; - JointTrajPt value; - - LOG_COMM("Executing joint trajectory unload"); - - rtn = buffer->unload(this->size_); - - if(rtn) - { - for (int i = this->size() - 1; i >= 0; i--) - { - rtn = buffer->unload(value); - if (!rtn) - { - LOG_ERROR("Failed to unload message point: %d from data[%d]", i, buffer->getBufferSize()); - break; - } - this->points_[i].copyFrom(value); - } - } - else - { - LOG_ERROR("Failed to unload trajectory size"); - } - return rtn; -} - -} -} - diff --git a/training/supplements/src/industrial_core/simple_message/src/joint_traj_pt.cpp b/training/supplements/src/industrial_core/simple_message/src/joint_traj_pt.cpp deleted file mode 100644 index 0d58caef..00000000 --- a/training/supplements/src/industrial_core/simple_message/src/joint_traj_pt.cpp +++ /dev/null @@ -1,181 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2011, Southwest Research Institute - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in the - * documentation and/or other materials provided with the distribution. - * * Neither the name of the Southwest Research Institute, nor the names - * of its contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ -#ifndef FLATHEADERS -#include "simple_message/joint_traj_pt.h" -#include "simple_message/shared_types.h" -#include "simple_message/log_wrapper.h" -#else -#include "joint_traj_pt.h" -#include "shared_types.h" -#include "log_wrapper.h" -#endif - -using namespace industrial::joint_data; -using namespace industrial::shared_types; - -namespace industrial -{ -namespace joint_traj_pt -{ - -JointTrajPt::JointTrajPt(void) -{ - this->init(); -} -JointTrajPt::~JointTrajPt(void) -{ - -} - -void JointTrajPt::init() -{ - this->joint_position_.init(); - this->sequence_ = 0; - this->velocity_ = 0.0; - this->duration_ = 0.0; -} - -void JointTrajPt::init(shared_int sequence, JointData & position, shared_real velocity, shared_real duration) -{ - this->setJointPosition(position); - this->setSequence(sequence); - this->setVelocity(velocity); - this->setDuration(duration); -} - -void JointTrajPt::copyFrom(JointTrajPt &src) -{ - this->setSequence(src.getSequence()); - src.getJointPosition(this->joint_position_); - this->setVelocity(src.getVelocity()); - this->setDuration(src.getDuration()); -} - -bool JointTrajPt::operator==(JointTrajPt &rhs) -{ - return this->joint_position_ == rhs.joint_position_ && this->sequence_ == rhs.sequence_ - && this->velocity_ == rhs.velocity_ && this->duration_ == rhs.duration_; - -} - -bool JointTrajPt::load(industrial::byte_array::ByteArray *buffer) -{ - bool rtn = false; - - LOG_COMM("Executing joint trajectory point load"); - - if (buffer->load(this->sequence_)) - { - if (this->joint_position_.load(buffer)) - { - if (buffer->load(this->velocity_)) - { - if (buffer->load(this->duration_)) - { - LOG_COMM("Trajectory point successfully loaded"); - rtn = true; - } - else - { - rtn = false; - LOG_ERROR("Failed to load joint traj pt. duration"); - } - rtn = true; - } - else - { - rtn = false; - LOG_ERROR("Failed to load joint traj pt. velocity"); - } - - } - else - { - rtn = false; - LOG_ERROR("Failed to load joint traj. pt. position data"); - } - } - else - { - rtn = false; - LOG_ERROR("Failed to load joint traj. pt. sequence number"); - } - - return rtn; -} - -bool JointTrajPt::unload(industrial::byte_array::ByteArray *buffer) -{ - bool rtn = false; - - LOG_COMM("Executing joint traj. pt. unload"); - if (buffer->unload(this->duration_)) - { - if (buffer->unload(this->velocity_)) - { - if (this->joint_position_.unload(buffer)) - { - if (buffer->unload(this->sequence_)) - { - rtn = true; - LOG_COMM("Joint traj. pt successfully unloaded"); - } - else - { - LOG_ERROR("Failed to unload joint traj. pt. sequence number"); - rtn = false; - } - } - else - { - LOG_ERROR("Failed to unload joint traj. pt. position data"); - rtn = false; - } - - } - else - { - LOG_ERROR("Failed to unload joint traj. pt. velocity"); - rtn = false; - } - } - else - { - LOG_ERROR("Failed to unload joint traj. pt. duration"); - rtn = false; - } - - return rtn; -} - -} -} - diff --git a/training/supplements/src/industrial_core/simple_message/src/joint_traj_pt_full.cpp b/training/supplements/src/industrial_core/simple_message/src/joint_traj_pt_full.cpp deleted file mode 100644 index 82819275..00000000 --- a/training/supplements/src/industrial_core/simple_message/src/joint_traj_pt_full.cpp +++ /dev/null @@ -1,210 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2013, Southwest Research Institute - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in the - * documentation and/or other materials provided with the distribution. - * * Neither the name of the Southwest Research Institute, nor the names - * of its contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ -#ifndef FLATHEADERS -#include "simple_message/joint_traj_pt_full.h" -#include "simple_message/shared_types.h" -#include "simple_message/log_wrapper.h" -#else -#include "joint_traj_pt_full.h" -#include "shared_types.h" -#include "log_wrapper.h" -#endif - -using namespace industrial::joint_data; -using namespace industrial::shared_types; - -namespace industrial -{ -namespace joint_traj_pt_full -{ - -JointTrajPtFull::JointTrajPtFull(void) -{ - this->init(); -} -JointTrajPtFull::~JointTrajPtFull(void) -{ - -} - -void JointTrajPtFull::init() -{ - this->robot_id_ = 0; - this->sequence_ = 0; - this->valid_fields_ = 0; - this->time_ = 0.0; - this->positions_.init(); - this->velocities_.init(); - this->accelerations_.init(); -} - -void JointTrajPtFull::init(industrial::shared_types::shared_int robot_id, - industrial::shared_types::shared_int sequence, - industrial::shared_types::shared_int valid_fields, - industrial::shared_types::shared_real time, - industrial::joint_data::JointData & positions, - industrial::joint_data::JointData & velocities, - industrial::joint_data::JointData & accelerations) -{ - this->setRobotID(robot_id); - this->setSequence(sequence); - this->setTime(time); - this->setPositions(positions); - this->setVelocities(velocities); - this->setAccelerations(accelerations); - this->valid_fields_ = valid_fields; // must happen after others are set -} - -void JointTrajPtFull::copyFrom(JointTrajPtFull &src) -{ - this->setRobotID(src.getRobotID()); - this->setSequence(src.getSequence()); - src.getTime(this->time_); - src.getPositions(this->positions_); - src.getVelocities(this->velocities_); - src.getAccelerations(this->accelerations_); - this->valid_fields_ = src.valid_fields_; -} - -bool JointTrajPtFull::operator==(JointTrajPtFull &rhs) -{ - return this->robot_id_ == rhs.robot_id_ && - this->sequence_ == rhs.sequence_ && - this->valid_fields_ == rhs.valid_fields_ && - ( !is_valid(ValidFieldTypes::TIME) || (this->time_ == rhs.time_) ) && - ( !is_valid(ValidFieldTypes::POSITION) || (this->positions_ == rhs.positions_) ) && - ( !is_valid(ValidFieldTypes::VELOCITY) || (this->velocities_ == rhs.velocities_) ) && - ( !is_valid(ValidFieldTypes::ACCELERATION) || (this->accelerations_ == rhs.accelerations_) ); -} - -bool JointTrajPtFull::load(industrial::byte_array::ByteArray *buffer) -{ - LOG_COMM("Executing joint trajectory point load"); - - if (!buffer->load(this->robot_id_)) - { - LOG_ERROR("Failed to load joint traj pt. robot_id"); - return false; - } - - if (!buffer->load(this->sequence_)) - { - LOG_ERROR("Failed to load joint traj. pt. sequence number"); - return false; - } - - if (!buffer->load(this->valid_fields_)) - { - LOG_ERROR("Failed to load joint traj. pt. valid fields"); - return false; - } - - if (!buffer->load(this->time_)) - { - LOG_ERROR("Failed to load joint traj. pt. time"); - return false; - } - - if (!this->positions_.load(buffer)) - { - LOG_ERROR("Failed to load joint traj. pt. positions"); - return false; - } - - if (!this->velocities_.load(buffer)) - { - LOG_ERROR("Failed to load joint traj. pt. velocities"); - return false; - } - - if (!this->accelerations_.load(buffer)) - { - LOG_ERROR("Failed to load joint traj. pt. accelerations"); - return false; - } - - LOG_COMM("Trajectory point successfully loaded"); - return true; -} - -bool JointTrajPtFull::unload(industrial::byte_array::ByteArray *buffer) -{ - LOG_COMM("Executing joint traj. pt. unload"); - - if (!this->accelerations_.unload(buffer)) - { - LOG_ERROR("Failed to unload joint traj. pt. accelerations"); - return false; - } - - if (!this->velocities_.unload(buffer)) - { - LOG_ERROR("Failed to unload joint traj. pt. velocities"); - return false; - } - - if (!this->positions_.unload(buffer)) - { - LOG_ERROR("Failed to unload joint traj. pt. positions"); - return false; - } - - if (!buffer->unload(this->time_)) - { - LOG_ERROR("Failed to unload joint traj. pt. time"); - return false; - } - - if (!buffer->unload(this->valid_fields_)) - { - LOG_ERROR("Failed to unload joint traj. pt. valid fields"); - return false; - } - - if (!buffer->unload(this->sequence_)) - { - LOG_ERROR("Failed to unload joint traj. pt. sequence number"); - return false; - } - - if (!buffer->unload(this->robot_id_)) - { - LOG_ERROR("Faild to unload joint traj. pt. robot_id"); - return false; - } - - LOG_COMM("Joint traj. pt successfully unloaded"); - return true; -} - -} -} - diff --git a/training/supplements/src/industrial_core/simple_message/src/message_handler.cpp b/training/supplements/src/industrial_core/simple_message/src/message_handler.cpp deleted file mode 100644 index 637ec15c..00000000 --- a/training/supplements/src/industrial_core/simple_message/src/message_handler.cpp +++ /dev/null @@ -1,133 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2011, Southwest Research Institute - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in the - * documentation and/or other materials provided with the distribution. - * * Neither the name of the Southwest Research Institute, nor the names - * of its contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ -#ifndef FLATHEADERS -#include "simple_message/message_handler.h" -#include "simple_message/log_wrapper.h" -#else -#include "message_handler.h" -#include "log_wrapper.h" -#endif - -namespace industrial -{ -namespace message_handler -{ - -using namespace industrial::smpl_msg_connection; -using namespace industrial::simple_message; - -MessageHandler::MessageHandler(void) -{ - this->setConnection(NULL); - this->setMsgType(StandardMsgTypes::INVALID); -} - - -MessageHandler::~MessageHandler(void) -{ -} - - -bool MessageHandler::init(int msg_type, SmplMsgConnection* connection) -{ - bool rtn = false; - - if (StandardMsgTypes::INVALID != msg_type) - { - if (NULL != connection) - { - this->setConnection(connection); - this->setMsgType(msg_type); - rtn = true; - } - else - { - LOG_ERROR("Message connection is NULL"); - rtn = false; - } - } - else - { - LOG_ERROR("Message handler type: %d, not valid", msg_type); - rtn = false; - } - - return rtn; -} - - - -bool MessageHandler::callback(SimpleMessage & in) -{ - bool rtn = false; - - if (validateMsg(in)) - { - this->internalCB(in); - } - else - { - LOG_ERROR("Invalid message passed to callback"); - rtn = true; - } - - return rtn; -} - - -bool MessageHandler::validateMsg(SimpleMessage & in) -{ - bool rtn = false; - - if (in.validateMessage()) - { - if (in.getMessageType() == this->getMsgType()) - { - rtn = true; - } - else - { - LOG_WARN("Message type: %d, doesn't match handler type: %d", - in.getMessageType(), this->getMsgType()); - rtn = false; - } - } - else - { - LOG_WARN("Passed in message invalid"); - } - - return rtn; - -} - -} // namespace message_handler -} // namespace industrial diff --git a/training/supplements/src/industrial_core/simple_message/src/message_manager.cpp b/training/supplements/src/industrial_core/simple_message/src/message_manager.cpp deleted file mode 100644 index e307b857..00000000 --- a/training/supplements/src/industrial_core/simple_message/src/message_manager.cpp +++ /dev/null @@ -1,275 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2011, Southwest Research Institute - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in the - * documentation and/or other materials provided with the distribution. - * * Neither the name of the Southwest Research Institute, nor the names - * of its contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ -#ifndef FLATHEADERS -#include "simple_message/message_manager.h" -#include "simple_message/log_wrapper.h" -#include "simple_message/simple_message.h" -#else -#include "message_manager.h" -#include "log_wrapper.h" -#include "simple_message.h" -#endif - -#ifdef ROS -#include "ros/ros.h" -#else -#include "unistd.h" -#endif - -using namespace industrial::smpl_msg_connection; -using namespace industrial::message_handler; -using namespace industrial::simple_message; -using namespace industrial::comms_fault_handler; -using namespace industrial::simple_comms_fault_handler; - -namespace industrial -{ -namespace message_manager -{ - -/** - * \brief Constructor - */ -MessageManager::MessageManager() -{ - this->num_handlers_ = 0; - for (unsigned int i = 0; i < this->getMaxNumHandlers(); i++) - { - this->handlers_[i] = NULL; - } - this->comms_hndlr_ = NULL; -} - -MessageManager::~MessageManager() -{ - -} - -bool MessageManager::init(SmplMsgConnection* connection) -{ - bool rtn = false; - - LOG_INFO("Initializing message manager with default comms fault handler"); - - - if (NULL != connection) - { - this->getDefaultCommsFaultHandler().init(connection); - this->init(connection, (CommsFaultHandler*)(&this->getDefaultCommsFaultHandler())); - rtn = true; - } - else - { - LOG_ERROR("NULL connection passed into manager init"); - rtn = false; - } - - return rtn; -} - -bool MessageManager::init(SmplMsgConnection* connection, CommsFaultHandler* fault_handler) -{ - bool rtn = false; - - LOG_INFO("Initializing message manager"); - - if (NULL != connection && NULL != fault_handler) - { - this->setConnection(connection); - this->getPingHandler().init(connection); - this->setCommsFaultHandler(fault_handler); - - if (this->add(&this->getPingHandler())) - { - rtn = true; - } - else - { - rtn = false; - LOG_WARN("Failed to add ping handler, manager won't respond to pings"); - } - - } - else - { - LOG_ERROR("NULL connection or NULL fault handler passed into manager init"); - rtn = false; - } - - return rtn; - } - - - -void MessageManager::spinOnce() -{ - SimpleMessage msg; - MessageHandler* handler = NULL; - - if(!this->getConnection()->isConnected()) - { - this->getCommsFaultHandler()->connectionFailCB(); - } - - if (this->getConnection()->receiveMsg(msg)) - { - LOG_COMM("Message received"); - handler = this->getHandler(msg.getMessageType()); - - if (NULL != handler) - { - LOG_DEBUG("Executing handler callback for message type: %d", handler->getMsgType()); - handler->callback(msg); - } - else - { - if (CommTypes::SERVICE_REQUEST == msg.getCommType()) - { - simple_message::SimpleMessage fail; - fail.init(msg.getMessageType(), CommTypes::SERVICE_REPLY, ReplyTypes::FAILURE); - this->getConnection()->sendMsg(fail); - LOG_WARN("Unhandled message type encounters, sending failure reply"); - } - LOG_ERROR("Message callback for message type: %d, not executed", msg.getMessageType()); - } - } - else - { - LOG_ERROR("Failed to receive incoming message"); - this->getCommsFaultHandler()->sendFailCB(); - } -} - -int ms_per_clock; -void mySleep(int sec) -{ -#ifdef MOTOPLUS - if (ms_per_clock <= 0) - ms_per_clock = mpGetRtc(); - - mpTaskDelay(sec * 1000 / ms_per_clock); -#else - sleep(sec); -#endif -} - -void MessageManager::spin() -{ - LOG_INFO("Entering message manager spin loop"); -#ifdef ROS - while (ros::ok()) -#else - while (true) -#endif - { - this->spinOnce(); - - // Throttle loop speed if waiting for a re-connection - if (!this->getConnection()->isConnected()) - mySleep(5); - } -} - -bool MessageManager::add(MessageHandler * handler, bool allow_replace) -{ - int idx = -1; - bool rtn = false; - - if (NULL != handler) - { - // If get handler returns -1 then a hander for the message type - // does not exist, and a new one should be added - idx = getHandlerIdx(handler->getMsgType()); - if (0 > idx) - { - if (this->getMaxNumHandlers() > this->getNumHandlers()) - { - this->handlers_[this->getNumHandlers()] = handler; - this->setNumHandlers(this->getNumHandlers() + 1); - LOG_INFO("Added message handler for message type: %d", handler->getMsgType()); - rtn = true; - } - else - { - LOG_ERROR("Max number of handlers exceeded"); - rtn = false; - } - } - else if (allow_replace) - { - this->handlers_[idx] = handler; - } - else - { - LOG_ERROR("Failed to add handler for: %d, handler already exists", handler->getMsgType()); - rtn = false; - } - } - else - { - LOG_ERROR("NULL handler not added"); - rtn = false; - } - return rtn; -} - -MessageHandler* MessageManager::getHandler(int msg_type) -{ - int idx = getHandlerIdx(msg_type); - - if (idx < 0) - return NULL; - else - return this->handlers_[idx]; -} - -int MessageManager::getHandlerIdx(int msg_type) -{ - int rtn = -1; - MessageHandler* temp = NULL; - - for (unsigned int i = 0; i < this->getMaxNumHandlers(); i++) - { - temp = this->handlers_[i]; - if (NULL == temp) break; // end of handler-list - - if (temp->getMsgType() == msg_type) - { - rtn = i; - break; - } - } - - return rtn; -} - -} // namespace message_manager -} // namespace industrial diff --git a/training/supplements/src/industrial_core/simple_message/src/messages/joint_feedback_message.cpp b/training/supplements/src/industrial_core/simple_message/src/messages/joint_feedback_message.cpp deleted file mode 100644 index f26e7a47..00000000 --- a/training/supplements/src/industrial_core/simple_message/src/messages/joint_feedback_message.cpp +++ /dev/null @@ -1,127 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2013, Southwest Research Institute - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in the - * documentation and/or other materials provided with the distribution. - * * Neither the name of the Southwest Research Institute, nor the names - * of its contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ -#ifndef FLATHEADERS -#include "simple_message/messages/joint_feedback_message.h" -#include "simple_message/joint_data.h" -#include "simple_message/byte_array.h" -#include "simple_message/log_wrapper.h" -#else -#include "joint_feedback_message.h" -#include "joint_data.h" -#include "byte_array.h" -#include "log_wrapper.h" -#endif - -using namespace industrial::shared_types; -using namespace industrial::byte_array; -using namespace industrial::simple_message; -using namespace industrial::joint_feedback; - -namespace industrial -{ -namespace joint_feedback_message -{ - -JointFeedbackMessage::JointFeedbackMessage(void) -{ - this->init(); -} - -JointFeedbackMessage::~JointFeedbackMessage(void) -{ - -} - -bool JointFeedbackMessage::init(industrial::simple_message::SimpleMessage & msg) -{ - bool rtn = false; - ByteArray data = msg.getData(); - this->init(); - - if (data.unload(this->data_)) - { - rtn = true; - } - else - { - LOG_ERROR("Failed to unload joint feedback message data"); - } - return rtn; -} - -void JointFeedbackMessage::init(industrial::joint_feedback::JointFeedback & data) -{ - this->init(); - this->data_.copyFrom(data); -} - -void JointFeedbackMessage::init() -{ - this->setMessageType(StandardMsgTypes::JOINT_FEEDBACK); - this->data_.init(); -} - -bool JointFeedbackMessage::load(ByteArray *buffer) -{ - bool rtn = false; - LOG_COMM("Executing joint feedback message load"); - if (buffer->load(this->data_)) - { - rtn = true; - } - else - { - rtn = false; - LOG_ERROR("Failed to load joint feedback message data"); - } - return rtn; -} - -bool JointFeedbackMessage::unload(ByteArray *buffer) -{ - bool rtn = false; - LOG_COMM("Executing joint feedback message unload"); - - if (buffer->unload(this->data_)) - { - rtn = true; - } - else - { - rtn = false; - LOG_ERROR("Failed to unload joint feedback message data"); - } - return rtn; -} - -} -} - diff --git a/training/supplements/src/industrial_core/simple_message/src/messages/joint_message.cpp b/training/supplements/src/industrial_core/simple_message/src/messages/joint_message.cpp deleted file mode 100644 index 0f777e9e..00000000 --- a/training/supplements/src/industrial_core/simple_message/src/messages/joint_message.cpp +++ /dev/null @@ -1,161 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2011, Southwest Research Institute - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in the - * documentation and/or other materials provided with the distribution. - * * Neither the name of the Southwest Research Institute, nor the names - * of its contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef FLATHEADERS -#include "simple_message/messages/joint_message.h" -#include "simple_message/joint_data.h" -#include "simple_message/byte_array.h" -#include "simple_message/log_wrapper.h" -#else -#include "joint_message.h" -#include "joint_data.h" -#include "byte_array.h" -#include "log_wrapper.h" -#endif - -using namespace industrial::shared_types; -using namespace industrial::byte_array; -using namespace industrial::simple_message; -using namespace industrial::joint_data; - -namespace industrial -{ -namespace joint_message -{ - -JointMessage::JointMessage(void) -{ - this->setMessageType(StandardMsgTypes::JOINT_POSITION); - this->init(); -} - -JointMessage::~JointMessage(void) -{ - -} - -bool JointMessage::init(industrial::simple_message::SimpleMessage & msg) -{ - bool rtn = false; - ByteArray data = msg.getData(); - - this->setMessageType(StandardMsgTypes::JOINT_POSITION); - - if (data.unload(this->joints_)) - { - if (data.unload(this->sequence_)) - { - rtn = true; - } - else - { - rtn = false; - LOG_ERROR("Failed to unload sequence data"); - } - } - else - { - LOG_ERROR("Failed to unload joint data"); - } - return rtn; -} - -void JointMessage::setSequence(shared_int sequence) -{ - this->sequence_ = sequence; -} - -void JointMessage::init(shared_int seq, JointData& joints) -{ - this->setSequence(seq); - this->joints_.copyFrom(joints); -} - -void JointMessage::init() -{ - this->setSequence(0); - this->joints_.init(); -} - -bool JointMessage::load(ByteArray *buffer) -{ - bool rtn = false; - LOG_COMM("Executing joint message load"); - if (buffer->load(this->getSequence())) - { - - if (buffer->load(this->joints_)) - { - rtn = true; - } - else - { - rtn = false; - LOG_ERROR("Failed to load sequence data"); - } - } - else - { - rtn = false; - LOG_ERROR("Failed to load sequence data"); - } - return rtn; -} - -bool JointMessage::unload(ByteArray *buffer) -{ - bool rtn = false; - LOG_COMM("Executing joint message unload"); - - if (buffer->unload(this->joints_)) - { - - if (buffer->unload(this->sequence_)) - { - rtn = true; - } - else - { - rtn = false; - LOG_ERROR("Failed to unload sequence data"); - } - } - else - { - rtn = false; - LOG_ERROR("Failed to unload joint data"); - } - return rtn; -} - -} -} - diff --git a/training/supplements/src/industrial_core/simple_message/src/messages/joint_traj_pt_full_message.cpp b/training/supplements/src/industrial_core/simple_message/src/messages/joint_traj_pt_full_message.cpp deleted file mode 100644 index e14e9c9f..00000000 --- a/training/supplements/src/industrial_core/simple_message/src/messages/joint_traj_pt_full_message.cpp +++ /dev/null @@ -1,129 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2013, Southwest Research Institute - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in the - * documentation and/or other materials provided with the distribution. - * * Neither the name of the Southwest Research Institute, nor the names - * of its contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef FLATHEADERS -#include "simple_message/messages/joint_traj_pt_full_message.h" -#include "simple_message/joint_data.h" -#include "simple_message/byte_array.h" -#include "simple_message/log_wrapper.h" -#else -#include "joint_traj_pt_full_message.h" -#include "joint_data.h" -#include "byte_array.h" -#include "log_wrapper.h" -#endif - -using namespace industrial::shared_types; -using namespace industrial::byte_array; -using namespace industrial::simple_message; -using namespace industrial::joint_traj_pt_full; - -namespace industrial -{ -namespace joint_traj_pt_full_message -{ - -JointTrajPtFullMessage::JointTrajPtFullMessage(void) -{ - this->init(); -} - -JointTrajPtFullMessage::~JointTrajPtFullMessage(void) -{ - -} - -bool JointTrajPtFullMessage::init(industrial::simple_message::SimpleMessage & msg) -{ - bool rtn = false; - ByteArray data = msg.getData(); - this->init(); - - if (data.unload(this->point_)) - { - rtn = true; - } - else - { - LOG_ERROR("Failed to unload joint traj pt data"); - } - return rtn; -} - -void JointTrajPtFullMessage::init(industrial::joint_traj_pt_full::JointTrajPtFull & point) -{ - this->init(); - this->point_.copyFrom(point); -} - -void JointTrajPtFullMessage::init() -{ - this->setMessageType(StandardMsgTypes::JOINT_TRAJ_PT_FULL); - this->point_.init(); -} - - -bool JointTrajPtFullMessage::load(ByteArray *buffer) -{ - bool rtn = false; - LOG_COMM("Executing joint traj. pt. message load"); - if (buffer->load(this->point_)) - { - rtn = true; - } - else - { - rtn = false; - LOG_ERROR("Failed to load joint traj. pt data"); - } - return rtn; -} - -bool JointTrajPtFullMessage::unload(ByteArray *buffer) -{ - bool rtn = false; - LOG_COMM("Executing joint traj pt message unload"); - - if (buffer->unload(this->point_)) - { - rtn = true; - } - else - { - rtn = false; - LOG_ERROR("Failed to unload joint traj pt data"); - } - return rtn; -} - -} -} - diff --git a/training/supplements/src/industrial_core/simple_message/src/messages/joint_traj_pt_message.cpp b/training/supplements/src/industrial_core/simple_message/src/messages/joint_traj_pt_message.cpp deleted file mode 100644 index ede0df10..00000000 --- a/training/supplements/src/industrial_core/simple_message/src/messages/joint_traj_pt_message.cpp +++ /dev/null @@ -1,129 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2011, Southwest Research Institute - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in the - * documentation and/or other materials provided with the distribution. - * * Neither the name of the Southwest Research Institute, nor the names - * of its contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef FLATHEADERS -#include "simple_message/messages/joint_traj_pt_message.h" -#include "simple_message/joint_data.h" -#include "simple_message/byte_array.h" -#include "simple_message/log_wrapper.h" -#else -#include "joint_traj_pt_message.h" -#include "joint_data.h" -#include "byte_array.h" -#include "log_wrapper.h" -#endif - -using namespace industrial::shared_types; -using namespace industrial::byte_array; -using namespace industrial::simple_message; -using namespace industrial::joint_traj_pt; - -namespace industrial -{ -namespace joint_traj_pt_message -{ - -JointTrajPtMessage::JointTrajPtMessage(void) -{ - this->init(); -} - -JointTrajPtMessage::~JointTrajPtMessage(void) -{ - -} - -bool JointTrajPtMessage::init(industrial::simple_message::SimpleMessage & msg) -{ - bool rtn = false; - ByteArray data = msg.getData(); - this->init(); - - if (data.unload(this->point_)) - { - rtn = true; - } - else - { - LOG_ERROR("Failed to unload joint traj pt data"); - } - return rtn; -} - -void JointTrajPtMessage::init(industrial::joint_traj_pt::JointTrajPt & point) -{ - this->init(); - this->point_.copyFrom(point); -} - -void JointTrajPtMessage::init() -{ - this->setMessageType(StandardMsgTypes::JOINT_TRAJ_PT); - this->point_.init(); -} - - -bool JointTrajPtMessage::load(ByteArray *buffer) -{ - bool rtn = false; - LOG_COMM("Executing joint traj. pt. message load"); - if (buffer->load(this->point_)) - { - rtn = true; - } - else - { - rtn = false; - LOG_ERROR("Failed to load joint traj. pt data"); - } - return rtn; -} - -bool JointTrajPtMessage::unload(ByteArray *buffer) -{ - bool rtn = false; - LOG_COMM("Executing joint traj pt message unload"); - - if (buffer->unload(this->point_)) - { - rtn = true; - } - else - { - rtn = false; - LOG_ERROR("Failed to unload joint traj pt data"); - } - return rtn; -} - -} -} - diff --git a/training/supplements/src/industrial_core/simple_message/src/messages/robot_status_message.cpp b/training/supplements/src/industrial_core/simple_message/src/messages/robot_status_message.cpp deleted file mode 100644 index 0c270db4..00000000 --- a/training/supplements/src/industrial_core/simple_message/src/messages/robot_status_message.cpp +++ /dev/null @@ -1,128 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2012, Southwest Research Institute - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in the - * documentation and/or other materials provided with the distribution. - * * Neither the name of the Southwest Research Institute, nor the names - * of its contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef FLATHEADERS -#include "simple_message/messages/robot_status_message.h" -#include "simple_message/robot_status.h" -#include "simple_message/byte_array.h" -#include "simple_message/log_wrapper.h" -#else -#include "robot_status_message.h" -#include "robot_status.h" -#include "byte_array.h" -#include "log_wrapper.h" -#endif - -using namespace industrial::shared_types; -using namespace industrial::byte_array; -using namespace industrial::simple_message; -using namespace industrial::robot_status; - -namespace industrial -{ -namespace robot_status_message -{ - -RobotStatusMessage::RobotStatusMessage(void) -{ - this->init(); -} - -RobotStatusMessage::~RobotStatusMessage(void) -{ - -} - -bool RobotStatusMessage::init(industrial::simple_message::SimpleMessage & msg) -{ - bool rtn = false; - ByteArray data = msg.getData(); - this->init(); - - if (data.unload(this->status_)) - { - rtn = true; - } - else - { - LOG_ERROR("Failed to unload robot status data"); - } - return rtn; -} - -void RobotStatusMessage::init(industrial::robot_status::RobotStatus & status) -{ - this->init(); - this->status_.copyFrom(status); -} - -void RobotStatusMessage::init() -{ - this->setMessageType(StandardMsgTypes::STATUS); - this->status_.init(); -} - -bool RobotStatusMessage::load(ByteArray *buffer) -{ - bool rtn = false; - LOG_COMM("Executing robot status message load"); - if (buffer->load(this->status_)) - { - rtn = true; - } - else - { - rtn = false; - LOG_ERROR("Failed to load robot status data"); - } - return rtn; -} - -bool RobotStatusMessage::unload(ByteArray *buffer) -{ - bool rtn = false; - LOG_COMM("Executing robot status message unload"); - - if (buffer->unload(this->status_)) - { - rtn = true; - } - else - { - rtn = false; - LOG_ERROR("Failed to unload robot status data"); - } - return rtn; -} - -} -} - diff --git a/training/supplements/src/industrial_core/simple_message/src/ping_handler.cpp b/training/supplements/src/industrial_core/simple_message/src/ping_handler.cpp deleted file mode 100644 index 17d29d9f..00000000 --- a/training/supplements/src/industrial_core/simple_message/src/ping_handler.cpp +++ /dev/null @@ -1,98 +0,0 @@ -/* -* Software License Agreement (BSD License) -* -* Copyright (c) 2011, Southwest Research Institute -* All rights reserved. -* -* Redistribution and use in source and binary forms, with or without -* modification, are permitted provided that the following conditions are met: -* -* * Redistributions of source code must retain the above copyright -* notice, this list of conditions and the following disclaimer. -* * Redistributions in binary form must reproduce the above copyright -* notice, this list of conditions and the following disclaimer in the -* documentation and/or other materials provided with the distribution. -* * Neither the name of the Southwest Research Institute, nor the names -* of its contributors may be used to endorse or promote products derived -* from this software without specific prior written permission. -* -* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE -* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE -* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR -* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF -* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS -* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN -* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) -* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -* POSSIBILITY OF SUCH DAMAGE. -*/ -#ifndef FLATHEADERS -#include "simple_message/ping_handler.h" -#include "simple_message/ping_message.h" -#include "simple_message/log_wrapper.h" -#else -#include "ping_handler.h" -#include "ping_message.h" -#include "log_wrapper.h" -#endif - -using namespace industrial::ping_message; -using namespace industrial::simple_message; - -namespace industrial -{ -namespace ping_handler -{ - - -bool PingHandler::init(industrial::smpl_msg_connection::SmplMsgConnection* connection) -{ - return this->init(StandardMsgTypes::PING, connection); -} - -bool PingHandler::internalCB(industrial::simple_message::SimpleMessage & in) -{ - bool rtn = false; - PingMessage ping; - SimpleMessage msg; - - if (ping.init(in)) - { - if (ping.toReply(msg, ReplyTypes::SUCCESS)) - { - if(this->getConnection()->sendMsg(msg)) - { - LOG_INFO("Ping return sent"); - rtn = true; - } - else - { - LOG_ERROR("Failed to send ping return"); - rtn = false; - } - } - else - { - LOG_ERROR("Failed to generate ping reply message"); - rtn = false; - } - } - else - { - LOG_ERROR("Failed to initialize ping message"); - rtn = false; - } - - - return rtn; -} - - - -}//namespace ping_handler -}//namespace industrial - - - diff --git a/training/supplements/src/industrial_core/simple_message/src/ping_message.cpp b/training/supplements/src/industrial_core/simple_message/src/ping_message.cpp deleted file mode 100644 index ca521456..00000000 --- a/training/supplements/src/industrial_core/simple_message/src/ping_message.cpp +++ /dev/null @@ -1,91 +0,0 @@ -/* -* Software License Agreement (BSD License) -* -* Copyright (c) 2011, Southwest Research Institute -* All rights reserved. -* -* Redistribution and use in source and binary forms, with or without -* modification, are permitted provided that the following conditions are met: -* -* * Redistributions of source code must retain the above copyright -* notice, this list of conditions and the following disclaimer. -* * Redistributions in binary form must reproduce the above copyright -* notice, this list of conditions and the following disclaimer in the -* documentation and/or other materials provided with the distribution. -* * Neither the name of the Southwest Research Institute, nor the names -* of its contributors may be used to endorse or promote products derived -* from this software without specific prior written permission. -* -* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE -* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE -* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR -* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF -* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS -* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN -* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) -* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -* POSSIBILITY OF SUCH DAMAGE. -*/ -#ifndef FLATHEADERS -#include "simple_message/ping_message.h" -#include "simple_message/log_wrapper.h" -#include "simple_message/byte_array.h" -#else -#include "ping_message.h" -#include "log_wrapper.h" -#include "byte_array.h" -#endif - -using namespace industrial::simple_message; -using namespace industrial::byte_array; - -namespace industrial -{ -namespace ping_message -{ - - -PingMessage::PingMessage(void) -{ - this->init(); -} - -PingMessage::~PingMessage(void) -{ - -} - - -bool PingMessage::init(SimpleMessage & msg) -{ - bool rtn = false; - - if (this->getMessageType() == msg.getMessageType()) - { - rtn = true; - } - else - { - LOG_ERROR("Failed to initialize message, wrong type: %d, expected %d", - msg.getMessageType(), this->getMessageType()); - rtn = false; - } - - return rtn; -} - - -void PingMessage::init() -{ - this->setMessageType(StandardMsgTypes::PING); -} - - -}// ping_message -}// industrial - - - - diff --git a/training/supplements/src/industrial_core/simple_message/src/robot_status.cpp b/training/supplements/src/industrial_core/simple_message/src/robot_status.cpp deleted file mode 100644 index fe24eee4..00000000 --- a/training/supplements/src/industrial_core/simple_message/src/robot_status.cpp +++ /dev/null @@ -1,205 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2011, Southwest Research Institute - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in the - * documentation and/or other materials provided with the distribution. - * * Neither the name of the Southwest Research Institute, nor the names - * of its contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ -#ifndef FLATHEADERS -#include "simple_message/robot_status.h" -#include "simple_message/shared_types.h" -#include "simple_message/log_wrapper.h" -#else -#include "robot_status.h" -#include "shared_types.h" -#include "log_wrapper.h" -#endif - -#ifdef ROS -// Files below used to translate between ROS messages enums and -// enums defined in this file -#include "industrial_msgs/RobotMode.h" -#include "industrial_msgs/TriState.h" -#endif - -using namespace industrial::shared_types; - -namespace industrial -{ -namespace robot_status -{ - -namespace RobotModes -{ - -#ifdef ROS - -int toROSMsgEnum(RobotModes::RobotMode mode) -{ - - switch (mode) - { - case RobotModes::AUTO: - return industrial_msgs::RobotMode::AUTO; - break; - case RobotModes::MANUAL: - return industrial_msgs::RobotMode::MANUAL; - break; - case RobotModes::UNKNOWN: - return industrial_msgs::RobotMode::UNKNOWN; - } - return industrial_msgs::RobotMode::UNKNOWN; - -} -; - -#endif - -} - -namespace TriStates -{ - -#ifdef ROS - -int toROSMsgEnum(TriStates::TriState state) -{ - - switch (state) - { - case TriStates::TS_UNKNOWN: - return industrial_msgs::TriState::UNKNOWN; - break; - case TriStates::TS_TRUE: - return industrial_msgs::TriState::TRUE; - break; - case TriStates::TS_FALSE: - return industrial_msgs::TriState::FALSE; - break; - } - return industrial_msgs::TriState::UNKNOWN; - -} -; - -#endif - -} - -RobotStatus::RobotStatus(void) -{ - this->init(); -} -RobotStatus::~RobotStatus(void) -{ - -} - -void RobotStatus::init() -{ - this->init(TriStates::TS_UNKNOWN, TriStates::TS_UNKNOWN, 0, TriStates::TS_UNKNOWN, - TriStates::TS_UNKNOWN, RobotModes::UNKNOWN, TriStates::TS_UNKNOWN); -} - -void RobotStatus::init(TriState drivesPowered, TriState eStopped, industrial::shared_types::shared_int errorCode, - TriState inError, TriState inMotion, RobotMode mode, TriState motionPossible) -{ - this->setDrivesPowered(drivesPowered); - this->setEStopped(eStopped); - this->setErrorCode(errorCode); - this->setInError(inError); - this->setInMotion(inMotion); - this->setMode(mode); - this->setMotionPossible(motionPossible); -} - -void RobotStatus::copyFrom(RobotStatus &src) -{ - this->setDrivesPowered(src.getDrivesPowered()); - this->setEStopped(src.getEStopped()); - this->setErrorCode(src.getErrorCode()); - this->setInError(src.getInError()); - this->setInMotion(src.getInMotion()); - this->setMode(src.getMode()); - this->setMotionPossible(src.getMotionPossible()); -} - -bool RobotStatus::operator==(RobotStatus &rhs) -{ - return this->drives_powered_ == rhs.drives_powered_ && this->e_stopped_ == rhs.e_stopped_ - && this->error_code_ == rhs.error_code_ && this->in_error_ == rhs.in_error_ && this->in_motion_ == rhs.in_motion_ - && this->mode_ == rhs.mode_ && this->motion_possible_ == rhs.motion_possible_; -} - -bool RobotStatus::load(industrial::byte_array::ByteArray *buffer) -{ - bool rtn = false; - - LOG_COMM("Executing robot status load"); - - if (buffer->load(this->drives_powered_) && buffer->load(this->e_stopped_) && buffer->load(this->error_code_) - && buffer->load(this->in_error_) && buffer->load(this->in_motion_) && buffer->load(this->mode_) - && buffer->load(this->motion_possible_)) - { - - LOG_COMM("Robot status successfully loaded"); - rtn = true; - } - else - { - LOG_COMM("Robot status not loaded"); - rtn = false; - } - - return rtn; -} - -bool RobotStatus::unload(industrial::byte_array::ByteArray *buffer) -{ - bool rtn = false; - - LOG_COMM("Executing robot status unload"); - if (buffer->unload(this->motion_possible_) && buffer->unload(this->mode_) && buffer->unload(this->in_motion_) - && buffer->unload(this->in_error_) && buffer->unload(this->error_code_) && buffer->unload(this->e_stopped_) - && buffer->unload(this->drives_powered_)) - { - - rtn = true; - LOG_COMM("Robot status successfully unloaded"); - } - - else - { - LOG_ERROR("Failed to unload robot status"); - rtn = false; - } - - return rtn; -} - -} -} - diff --git a/training/supplements/src/industrial_core/simple_message/src/simple_comms_fault_handler.cpp b/training/supplements/src/industrial_core/simple_message/src/simple_comms_fault_handler.cpp deleted file mode 100644 index 6c7d48be..00000000 --- a/training/supplements/src/industrial_core/simple_message/src/simple_comms_fault_handler.cpp +++ /dev/null @@ -1,94 +0,0 @@ -/* -* Software License Agreement (BSD License) -* -* Copyright (c) 2011, Southwest Research Institute -* All rights reserved. -* -* Redistribution and use in source and binary forms, with or without -* modification, are permitted provided that the following conditions are met: -* -* * Redistributions of source code must retain the above copyright -* notice, this list of conditions and the following disclaimer. -* * Redistributions in binary form must reproduce the above copyright -* notice, this list of conditions and the following disclaimer in the -* documentation and/or other materials provided with the distribution. -* * Neither the name of the Southwest Research Institute, nor the names -* of its contributors may be used to endorse or promote products derived -* from this software without specific prior written permission. -* -* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE -* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE -* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR -* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF -* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS -* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN -* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) -* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -* POSSIBILITY OF SUCH DAMAGE. -*/ - -#ifndef FLATHEADERS -#include "simple_message/simple_comms_fault_handler.h" -#include "simple_message/log_wrapper.h" -#else -#include "simple_comms_fault_handler.h" -#include "log_wrapper.h" -#endif - -namespace industrial -{ -namespace simple_comms_fault_handler -{ - -SimpleCommsFaultHandler::SimpleCommsFaultHandler() -{ - this->connection_ = NULL; -} - - -SimpleCommsFaultHandler::~SimpleCommsFaultHandler() -{ -} - -bool SimpleCommsFaultHandler::init(industrial::smpl_msg_connection::SmplMsgConnection* connection) -{ - bool rtn = false; - - if (NULL != connection) - { - this->setConnection(connection); - LOG_INFO("Default communications fault handler successfully initialized"); - rtn = true; - } - else - { - LOG_ERROR("Failed to initialize default communications fault handler"); - rtn = false; - } - return rtn; -} - -void SimpleCommsFaultHandler::connectionFailCB() -{ - - if (!(this->getConnection()->isConnected())) - { - LOG_INFO("Connection failed, attempting reconnect"); - this->getConnection()->makeConnect(); - } - else - { - LOG_WARN("Connection fail callback called while still connected (Possible bug)"); - } -} - - - -}//namespace default_comms_fault_handler -}//namespace industrial - - - - diff --git a/training/supplements/src/industrial_core/simple_message/src/simple_message.cpp b/training/supplements/src/industrial_core/simple_message/src/simple_message.cpp deleted file mode 100755 index 083ff089..00000000 --- a/training/supplements/src/industrial_core/simple_message/src/simple_message.cpp +++ /dev/null @@ -1,168 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2011, Southwest Research Institute - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in the - * documentation and/or other materials provided with the distribution. - * * Neither the name of the Southwest Research Institute, nor the names - * of its contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef FLATHEADERS -#include "simple_message/simple_message.h" -#include "simple_message/log_wrapper.h" -#else -#include "simple_message.h" -#include "log_wrapper.h" -#endif - -#ifdef MOTOPLUS -#include "motoPlus.h" -#endif - - -using namespace industrial::byte_array; - -namespace industrial -{ - -namespace simple_message -{ - -SimpleMessage::SimpleMessage(void) -{ -} - -SimpleMessage::~SimpleMessage(void) -{ -} - - - -bool SimpleMessage::init(int msgType, int commType, int replyCode) -{ - ByteArray data; - data.init(); - return this->init(msgType, commType, replyCode, data); -} - -bool SimpleMessage::init(int msgType, int commType, int replyCode, ByteArray & data ) -{ - LOG_COMM("SimpleMessage::init(type: %d, comm: %d, reply: %d, data[%d]...)", - msgType, commType, replyCode, data.getBufferSize()); - this->setMessageType(msgType); - this->setCommType(commType); - this->setReplyCode(replyCode); - this->data_.copyFrom(data); - - return this->validateMessage(); -} - -bool SimpleMessage::init(ByteArray & msg) -{ - int dataSize = 0; - bool rtn = false; - - if (msg.getBufferSize() >= this->getHeaderSize()) - { - // Check to see if the message is larger than the standard header - // If so, copy out the data portion. - if (msg.getBufferSize() > this->getHeaderSize()) - { - dataSize = msg.getBufferSize() - this->getHeaderSize(); - LOG_COMM("Unloading data portion of message: %d bytes", dataSize); - msg.unload(this->data_, dataSize); - } - LOG_COMM("Unloading header data"); - msg.unload(this->reply_code_); - msg.unload(this->comm_type_); - msg.unload(this->message_type_); - LOG_COMM("SimpleMessage::init(type: %d, comm: %d, reply: %d, data[%d]...)", - this->message_type_, this->comm_type_, this->reply_code_, this->data_.getBufferSize()); - rtn = this->validateMessage(); - } - else - { - LOG_ERROR("Failed to init message, buffer size too small: %u", msg.getBufferSize()); - rtn = false; - } - return rtn; -} - -void SimpleMessage::toByteArray(ByteArray & msg) -{ - msg.init(); - - msg.load(this->getMessageType()); - msg.load(this->getCommType()); - msg.load(this->getReplyCode()); - if (this->data_.getBufferSize() > 0 ) - { - msg.load(this->getData().getRawDataPtr(), this->data_.getBufferSize()); - } - -} - - -void SimpleMessage::setData( ByteArray & data) -{ - this->data_.copyFrom(data); -} - - -bool SimpleMessage::validateMessage() -{ - - if ( StandardMsgTypes::INVALID == this->getMessageType()) - { - LOG_WARN("Invalid message type: %u", this->getMessageType()); - return false; - } - - if ( CommTypes::INVALID == this->getCommType()) - { - LOG_WARN("Invalid comms. type: %u", this->getCommType()); - return false; - } - - if ( - (CommTypes::SERVICE_REPLY == this->getCommType() && - ReplyTypes::INVALID == this->getReplyCode()) || - ((CommTypes::SERVICE_REPLY != this->getCommType() && - ReplyTypes::INVALID != this->getReplyCode())) - ) - { - LOG_WARN("Invalid reply. Comm type: %u, Reply type: %u", - this->getCommType(), this->getReplyCode()); - return false; - } - - return true; -} - - - - -} // namespace simple_message -} // namespace industrial diff --git a/training/supplements/src/industrial_core/simple_message/src/smpl_msg_connection.cpp b/training/supplements/src/industrial_core/simple_message/src/smpl_msg_connection.cpp deleted file mode 100755 index 3a605a1d..00000000 --- a/training/supplements/src/industrial_core/simple_message/src/smpl_msg_connection.cpp +++ /dev/null @@ -1,151 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2011, Southwest Research Institute - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in the - * documentation and/or other materials provided with the distribution. - * * Neither the name of the Southwest Research Institute, nor the names - * of its contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef FLATHEADERS -#include "simple_message/log_wrapper.h" -#include "simple_message/smpl_msg_connection.h" -#include "simple_message/byte_array.h" -#else -#include "log_wrapper.h" -#include "smpl_msg_connection.h" -#include "byte_array.h" -#endif - -#ifdef MOTOPLUS -#include "motoPlus.h" -#endif - -using namespace industrial::simple_message; -using namespace industrial::byte_array; - -namespace industrial -{ - -namespace smpl_msg_connection -{ - - -bool SmplMsgConnection::sendMsg(SimpleMessage & message) -{ - bool rtn; - ByteArray sendBuffer; - ByteArray msgData; - - if (message.validateMessage()) - { - message.toByteArray(msgData); - sendBuffer.load((int)msgData.getBufferSize()); - sendBuffer.load(msgData); - rtn = this->sendBytes(sendBuffer); - } - else - { - rtn = false; - LOG_ERROR("Message validation failed, message not sent"); - } - -return rtn; -} - - -bool SmplMsgConnection::receiveMsg(SimpleMessage & message) -{ - ByteArray lengthBuffer; - ByteArray msgBuffer; - int length; - - bool rtn = false; - - - rtn = this->receiveBytes(lengthBuffer, message.getLengthSize()); - - if (rtn) - { - rtn = lengthBuffer.unload(length); - LOG_COMM("Message length: %d", length); - - if (rtn) - { - rtn = this->receiveBytes(msgBuffer, length); - - if (rtn) - { - rtn = message.init(msgBuffer); - } - else - { - LOG_ERROR("Failed to initialize message"); - rtn = false; - } - - } - else - { - LOG_ERROR("Failed to receive message"); - rtn = false; - } - } - else - { - LOG_ERROR("Failed to receive message length"); - rtn = false; - } - - return rtn; -} - - - -bool SmplMsgConnection::sendAndReceiveMsg(SimpleMessage & send, SimpleMessage & recv, bool verbose) -{ - bool rtn = false; - rtn = this->sendMsg(send); - if (rtn) - { - if(verbose) { - LOG_ERROR("Sent message"); - } - rtn = this->receiveMsg(recv); - if(verbose) { - LOG_ERROR("Got message"); - } - } - else - { - rtn = false; - } - - return rtn; -} - - -}//smpl_msg_connection -}//industrial diff --git a/training/supplements/src/industrial_core/simple_message/src/socket/simple_socket.cpp b/training/supplements/src/industrial_core/simple_message/src/socket/simple_socket.cpp deleted file mode 100644 index 1772e7fe..00000000 --- a/training/supplements/src/industrial_core/simple_message/src/socket/simple_socket.cpp +++ /dev/null @@ -1,241 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2011, Yaskawa America, Inc. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in the - * documentation and/or other materials provided with the distribution. - * * Neither the name of the Yaskawa America, Inc., nor the names - * of its contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef FLATHEADERS -#include "simple_message/socket/simple_socket.h" -#include "simple_message/log_wrapper.h" -#else -#include "simple_socket.h" -#include "log_wrapper.h" -#endif - -using namespace industrial::byte_array; -using namespace industrial::shared_types; - -namespace industrial -{ - namespace simple_socket - { - - bool SimpleSocket::sendBytes(ByteArray & buffer) - { - int rc = this->SOCKET_FAIL; - bool rtn = false; - - if (this->isConnected()) - { - // Nothing restricts the ByteArray from being larger than the what the socket - // can handle. - if (this->MAX_BUFFER_SIZE > (int)buffer.getBufferSize()) - { - - rc = rawSendBytes(buffer.getRawDataPtr(), buffer.getBufferSize()); - if (this->SOCKET_FAIL != rc) - { - rtn = true; - } - else - { - rtn = false; - logSocketError("Socket sendBytes failed", rc); - } - - } - else - { - LOG_ERROR("Buffer size: %u, is greater than max socket size: %u", buffer.getBufferSize(), this->MAX_BUFFER_SIZE); - rtn = false; - } - - } - else - { - rtn = false; - LOG_WARN("Not connected, bytes not sent"); - } - - if (!rtn) - { - this->setConnected(false); - } - - return rtn; - - } - - bool SimpleSocket::receiveBytes(ByteArray & buffer, shared_int num_bytes) - { - int rc = this->SOCKET_FAIL; - bool rtn = false; - shared_int remainBytes = num_bytes; - bool ready, error; - - // Reset the buffer (this is not required since the buffer length should - // ensure that we don't read any of the garbage that may be left over from - // a previous read), but it is good practice. - - memset(&this->buffer_, 0, sizeof(this->buffer_)); - - // Doing a sanity check to determine if the byte array buffer is larger than - // what can be sent in the socket. This should not happen and might be indicative - // of some code synchronization issues between the client and server base. - if (this->MAX_BUFFER_SIZE < (int)buffer.getMaxBufferSize()) - { - LOG_WARN("Socket buffer max size: %u, is larger than byte array buffer: %u", - this->MAX_BUFFER_SIZE, buffer.getMaxBufferSize()); - } - if (this->isConnected()) - { - buffer.init(); - while (remainBytes > 0) - { - // Polling the socket results in an "interruptable" socket read. This - // allows Control-C to break out of a socket read. Without polling, - // a sig-term is required to kill a program in a socket read function. - if (this->poll(this->SOCKET_POLL_TO, ready, error)) - { - if(ready) - { - rc = rawReceiveBytes(this->buffer_, remainBytes); - if (this->SOCKET_FAIL == rc) - { - this->logSocketError("Socket received failed", rc); - remainBytes = 0; - rtn = false; - break; - } - else if (0 == rc) - { - LOG_WARN("Recieved zero bytes: %u", rc); - remainBytes = 0; - rtn = false; - break; - } - else - { - remainBytes = remainBytes - rc; - LOG_COMM("Byte array receive, bytes read: %u, bytes reqd: %u, bytes left: %u", - rc, num_bytes, remainBytes); - buffer.load(&this->buffer_, rc); - rtn = true; - } - } - else if(error) - { - LOG_ERROR("Socket poll returned an error"); - rtn = false; - break; - } - else - { - LOG_ERROR("Uknown error from socket poll"); - rtn = false; - break; - } - } - else - { - LOG_COMM("Socket poll timeout, trying again"); - } - } - } - else - { - LOG_WARN("Not connected, bytes not sent"); - rtn = false; - } - - if (!rtn) - { - this->setConnected(false); - } - return rtn; - } - - bool SimpleSocket::poll(int timeout, bool & ready, bool & error) - { - timeval time; - fd_set read, write, except; - int rc = this->SOCKET_FAIL; - bool rtn = false; - ready = false; - error = false; - - // The select function uses the timeval data structure - time.tv_sec = timeout / 1000; - time.tv_usec = (timeout % 1000) * 1000; - - FD_ZERO(&read); - FD_ZERO(&write); - FD_ZERO(&except); - - FD_SET(this->getSockHandle(), &read); - FD_SET(this->getSockHandle(), &except); - - rc = SELECT(this->getSockHandle() + 1, &read, &write, &except, &time); - - if (this->SOCKET_FAIL != rc) - { - if (0 == rc) - { - rtn = false; - } - else - { - if (FD_ISSET(this->getSockHandle(), &read)) - { - ready = true; - rtn = true; - } - else if(FD_ISSET(this->getSockHandle(), &except)) - { - error = true; - rtn = true; - } - else - { - LOG_WARN("Select returned, but no flags are set"); - rtn = false; - } - } - } - else - { - this->logSocketError("Socket select function failed", rc); - rtn = false; - } - - return rtn; - } - - } //simple_socket -} //industrial - diff --git a/training/supplements/src/industrial_core/simple_message/src/socket/tcp_client.cpp b/training/supplements/src/industrial_core/simple_message/src/socket/tcp_client.cpp deleted file mode 100644 index 60ff3961..00000000 --- a/training/supplements/src/industrial_core/simple_message/src/socket/tcp_client.cpp +++ /dev/null @@ -1,124 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2011, Southwest Research Institute - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in the - * documentation and/or other materials provided with the distribution. - * * Neither the name of the Southwest Research Institute, nor the names - * of its contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ -#ifndef FLATHEADERS -#include "simple_message/socket/tcp_client.h" -#include "simple_message/log_wrapper.h" -#else -#include "tcp_client.h" -#include "log_wrapper.h" -#endif - -namespace industrial -{ -namespace tcp_client -{ - -TcpClient::TcpClient() -{ - -} - -TcpClient::~TcpClient() -{ - LOG_DEBUG("Destructing TCPClient"); -} - -bool TcpClient::init(char *buff, int port_num) -{ - - int rc; - bool rtn; - int disableNodeDelay = 1; - - rc = SOCKET(AF_INET, SOCK_STREAM, 0); - if (this->SOCKET_FAIL != rc) - { - this->setSockHandle(rc); - - // The set no delay disables the NAGEL algorithm - rc = SET_NO_DELAY(this->getSockHandle(), disableNodeDelay); - if (this->SOCKET_FAIL == rc) - { - LOG_WARN("Failed to set no socket delay, sending data can be delayed by up to 250ms"); - } - - // Initialize address data structure - memset(&this->sockaddr_, 0, sizeof(this->sockaddr_)); - this->sockaddr_.sin_family = AF_INET; - this->sockaddr_.sin_addr.s_addr = INET_ADDR(buff); - this->sockaddr_.sin_port = HTONS(port_num); - - rtn = true; - - } - else - { - LOG_ERROR("Failed to create socket, rc: %d", rc); - rtn = false; - } - return rtn; -} - -bool TcpClient::makeConnect() -{ - bool rtn = false; - int rc = this->SOCKET_FAIL; - SOCKLEN_T addrSize = 0; - - if (!this->isConnected()) - { - addrSize = sizeof(this->sockaddr_); - rc = CONNECT(this->getSockHandle(), (sockaddr *)&this->sockaddr_, addrSize); - if (this->SOCKET_FAIL != rc) - { - LOG_INFO("Connected to server"); - this->setConnected(true); - rtn = true; - } - else - { - this->logSocketError("Failed to connect to server", rc); - rtn = false; - } - } - - else - { - LOG_WARN("Tried to connect when socket already in connected state"); - } - - return rtn; - -} - -} //tcp_client -} //industrial - diff --git a/training/supplements/src/industrial_core/simple_message/src/socket/tcp_server.cpp b/training/supplements/src/industrial_core/simple_message/src/socket/tcp_server.cpp deleted file mode 100644 index 72be6378..00000000 --- a/training/supplements/src/industrial_core/simple_message/src/socket/tcp_server.cpp +++ /dev/null @@ -1,169 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2011, Southwest Research Institute - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in the - * documentation and/or other materials provided with the distribution. - * * Neither the name of the Southwest Research Institute, nor the names - * of its contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ -#ifndef FLATHEADERS -#include "simple_message/socket/tcp_server.h" -#include "simple_message/log_wrapper.h" -#else -#include "tcp_server.h" -#include "log_wrapper.h" -#endif - -namespace industrial -{ -namespace tcp_server -{ - -TcpServer::TcpServer() -{ - this->setSockHandle(this->SOCKET_FAIL); - this->setSrvrHandle(this->SOCKET_FAIL); - memset(&this->sockaddr_, 0, sizeof(this->sockaddr_)); -} - -TcpServer::~TcpServer() -{ - CLOSE(this->getSockHandle()); - CLOSE(this->getSrvrHandle()); -} - -bool TcpServer::init(int port_num) -{ - int rc; - bool rtn; - const int reuse_addr = 1; - //int err; - SOCKLEN_T addrSize = 0; - - rc = SOCKET(AF_INET, SOCK_STREAM, 0); - if (this->SOCKET_FAIL != rc) - { - this->setSrvrHandle(rc); - LOG_DEBUG("Socket created, rc: %d", rc); - LOG_DEBUG("Socket handle: %d", this->getSrvrHandle()); - - - SET_REUSE_ADDR(this->getSrvrHandle(), reuse_addr); - - // Initialize address data structure - memset(&this->sockaddr_, 0, sizeof(this->sockaddr_)); - this->sockaddr_.sin_family = AF_INET; - this->sockaddr_.sin_addr.s_addr = INADDR_ANY; - this->sockaddr_.sin_port = HTONS(port_num); - - addrSize = sizeof(this->sockaddr_); - rc = BIND(this->getSrvrHandle(), (sockaddr *)&(this->sockaddr_), addrSize); - - if (this->SOCKET_FAIL != rc) - { - LOG_INFO("Server socket successfully initialized"); - - rc = LISTEN(this->getSrvrHandle(), 1); - - if (this->SOCKET_FAIL != rc) - { - LOG_INFO("Socket in listen mode"); - rtn = true; - } - else - { - LOG_ERROR("Failed to set socket to listen"); - rtn = false; - } - } - else - { - LOG_ERROR("Failed to bind socket, rc: %d", rc); - CLOSE(this->getSrvrHandle()); - rtn = false; - } - - } - else - { - LOG_ERROR("Failed to create socket, rc: %d", rc); - rtn = false; - } - - return rtn; -} - -bool TcpServer::makeConnect() -{ - bool rtn = false; - int rc = this->SOCKET_FAIL; - //int socket = this->SOCKET_FAIL; - int disableNodeDelay = 1; - int err = 0; - - if (!this->isConnected()) - { - this->setConnected(false); - if (this->SOCKET_FAIL != this->getSockHandle()) - { - CLOSE(this->getSockHandle()); - this->setSockHandle(this->SOCKET_FAIL); - } - - rc = ACCEPT(this->getSrvrHandle(), NULL, NULL); - - if (this->SOCKET_FAIL != rc) - { - this->setSockHandle(rc); - LOG_INFO("Client socket accepted"); - - // The set no delay disables the NAGEL algorithm - rc = SET_NO_DELAY(this->getSockHandle(), disableNodeDelay); - err = errno; - if (this->SOCKET_FAIL == rc) - { - LOG_WARN("Failed to set no socket delay, errno: %d, sending data can be delayed by up to 250ms", err); - } - this->setConnected(true); - rtn = true; - } - else - { - LOG_ERROR("Failed to accept for client connection"); - rtn = false; - } - } - else - { - LOG_WARN("Tried to connect when socket already in connected state"); - } - - return rtn; - -} - -} //tcp_socket -} //industrial - diff --git a/training/supplements/src/industrial_core/simple_message/src/socket/tcp_socket.cpp b/training/supplements/src/industrial_core/simple_message/src/socket/tcp_socket.cpp deleted file mode 100644 index 0977899c..00000000 --- a/training/supplements/src/industrial_core/simple_message/src/socket/tcp_socket.cpp +++ /dev/null @@ -1,91 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2011, Southwest Research Institute - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in the - * documentation and/or other materials provided with the distribution. - * * Neither the name of the Southwest Research Institute, nor the names - * of its contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef FLATHEADERS -#include "simple_message/socket/tcp_socket.h" -#include "simple_message/log_wrapper.h" -#include "simple_message/simple_message.h" -#include "simple_message/shared_types.h" -#else -#include "tcp_socket.h" -#include "log_wrapper.h" -#include "simple_message.h" -#include "shared_types.h" -#endif - -using namespace industrial::smpl_msg_connection; -using namespace industrial::byte_array; -using namespace industrial::simple_message; -using namespace industrial::shared_types; - -namespace industrial -{ -namespace tcp_socket -{ - -TcpSocket::TcpSocket() -// Constructor for UDP socket object -{ - this->setSockHandle(this->SOCKET_FAIL); - memset(&this->sockaddr_, 0, sizeof(this->sockaddr_)); - this->setConnected(false); - -} - -TcpSocket::~TcpSocket() -// Destructor for UDP socket object -// Closes socket -{ - LOG_DEBUG("Destructing TCPSocket"); - CLOSE(this->getSockHandle()); -} - -int TcpSocket::rawSendBytes(char *buffer, shared_int num_bytes) -{ - int rc = this->SOCKET_FAIL; - - rc = SEND(this->getSockHandle(), buffer, num_bytes, 0); - - return rc; -} - -int TcpSocket::rawReceiveBytes(char *buffer, shared_int num_bytes) -{ - int rc = this->SOCKET_FAIL; - - rc = RECV(this->getSockHandle(), buffer, num_bytes, 0); - - return rc; -} - -} //tcp_socket -} //industrial - diff --git a/training/supplements/src/industrial_core/simple_message/src/socket/udp_client.cpp b/training/supplements/src/industrial_core/simple_message/src/socket/udp_client.cpp deleted file mode 100644 index e2806d4f..00000000 --- a/training/supplements/src/industrial_core/simple_message/src/socket/udp_client.cpp +++ /dev/null @@ -1,134 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2011, Yaskawa America, Inc. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in the - * documentation and/or other materials provided with the distribution. - * * Neither the name of the Yaskawa America, Inc., nor the names - * of its contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ -#ifndef FLATHEADERS -#include "simple_message/socket/udp_client.h" -#include "simple_message/log_wrapper.h" -#else -#include "udp_client.h" -#include "log_wrapper.h" -#endif - - -using namespace industrial::byte_array; -using namespace industrial::shared_types; - -namespace industrial -{ -namespace udp_client -{ - -UdpClient::UdpClient() -{ -} - -UdpClient::~UdpClient() -{ -} - -bool UdpClient::init(char *buff, int port_num) -{ - - int rc; - bool rtn; - - /* Create a socket using: - * AF_INET - IPv4 internet protocol - * SOCK_DGRAM - UDP type - * protocol (0) - System chooses - */ - rc = SOCKET(AF_INET, SOCK_DGRAM, 0); - if (this->SOCKET_FAIL != rc) - { - this->setSockHandle(rc); - - // Initialize address data structure - memset(&this->sockaddr_, 0, sizeof(this->sockaddr_)); - this->sockaddr_.sin_family = AF_INET; - this->sockaddr_.sin_addr.s_addr = INET_ADDR(buff); - this->sockaddr_.sin_port = HTONS(port_num); - - rtn = true; - - } - else - { - LOG_ERROR("Failed to create socket, rc: %d", rc); - rtn = false; - } - return rtn; -} - - -bool UdpClient::makeConnect() -{ - ByteArray send; - char sendHS = this->CONNECT_HANDSHAKE; - char recvHS = 0; - bool rtn = false; - const int timeout = 1000; // Time (ms) between handshake sends - int bytesRcvd = 0; - - if (!this->isConnected()) - { - this->setConnected(false); - send.load((void*)&sendHS, sizeof(sendHS)); - - do - { - ByteArray recv; - recvHS = 0; - LOG_DEBUG("UDP client sending handshake"); - this->rawSendBytes(send.getRawDataPtr(), send.getBufferSize()); - if (this->isReadyReceive(timeout)) - { - bytesRcvd = this->rawReceiveBytes(this->buffer_, 0); - LOG_DEBUG("UDP client received possible handshake"); - recv.init(&this->buffer_[0], bytesRcvd); - recv.unload((void*)&recvHS, sizeof(recvHS)); - } - } - while(recvHS != sendHS); - LOG_INFO("UDP client connected"); - rtn = true; - this->setConnected(true); - - } - else - { - rtn = true; - LOG_WARN("Tried to connect when socket already in connected state"); - } - - return rtn; -} -} //udp_client -} //industrial - diff --git a/training/supplements/src/industrial_core/simple_message/src/socket/udp_server.cpp b/training/supplements/src/industrial_core/simple_message/src/socket/udp_server.cpp deleted file mode 100644 index 8e2bbfd4..00000000 --- a/training/supplements/src/industrial_core/simple_message/src/socket/udp_server.cpp +++ /dev/null @@ -1,159 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2011, Yaskawa America, Inc. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in the - * documentation and/or other materials provided with the distribution. - * * Neither the name of the Yaskawa America, Inc., nor the names - * of its contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef FLATHEADERS -#include "simple_message/socket/udp_server.h" -#include "simple_message/log_wrapper.h" -#else -#include "udp_server.h" -#include "log_wrapper.h" -#endif - -using namespace industrial::byte_array; - -namespace industrial -{ -namespace udp_server -{ - -UdpServer::UdpServer() -{ - this->setConnected(false); -} - -UdpServer::~UdpServer() -{ -} - - - -bool UdpServer::init(int port_num) -{ - int rc = this->SOCKET_FAIL; - bool rtn; - SOCKLEN_T addrSize = 0; - - /* Create a socket using: - * AF_INET - IPv4 internet protocol - * SOCK_DGRAM - UDP type - * protocol (0) - System chooses - */ - rc = SOCKET(AF_INET, SOCK_DGRAM, 0); - if (this->SOCKET_FAIL != rc) - { - this->setSockHandle(rc); - LOG_DEBUG("Socket created, rc: %d", rc); - LOG_DEBUG("Socket handle: %d", this->getSockHandle()); - - // Initialize address data structure - memset(&this->sockaddr_, 0, sizeof(this->sockaddr_)); - this->sockaddr_.sin_family = AF_INET; - this->sockaddr_.sin_addr.s_addr = INADDR_ANY; - this->sockaddr_.sin_port = HTONS(port_num); - - // This set the socket to be non-blocking (NOT SURE I WANT THIS) - sme - //fcntl(sock_handle, F_SETFL, O_NONBLOCK); - - addrSize = sizeof(this->sockaddr_); - rc = BIND(this->getSockHandle(), (sockaddr *)&(this->sockaddr_), addrSize); - - if (this->SOCKET_FAIL != rc) - { - rtn = true; - LOG_INFO("Server socket successfully initialized"); - } - else - { - LOG_ERROR("Failed to bind socket, rc: %d", rc); - CLOSE(this->getSockHandle()); - rtn = false; - } - } - else - { - LOG_ERROR("Failed to create socket, rc: %d", rc); - rtn = false; - } - return rtn; -} - - -bool UdpServer::makeConnect() -{ - ByteArray send; - char sendHS = this->CONNECT_HANDSHAKE; - char recvHS = 0; - int bytesRcvd = 0; - bool rtn = false; - - send.load((void*)&sendHS, sizeof(sendHS)); - - if (!this->isConnected()) - { - this->setConnected(false); - - // Listen for handshake. Once received, break - // listen loop. - do - { - ByteArray recv; - recvHS = 0; - bytesRcvd = this->rawReceiveBytes(this->buffer_, 0); - - if (bytesRcvd > 0) - { - LOG_DEBUG("UDP server received %d bytes while waiting for handshake", bytesRcvd); - recv.init(&this->buffer_[0], bytesRcvd); - recv.unload((void*)&recvHS, sizeof(recvHS)); - } - - } - while(recvHS != sendHS); - - // Send a reply handshake - this->rawSendBytes(send.getRawDataPtr(), send.getBufferSize()); - this->setConnected(true); - rtn = true; - - } - else - { - LOG_WARN("Tried to connect when socket already in connected state"); - rtn = true; - } - - return rtn; -} - - -} //udp_server -} //industrial - diff --git a/training/supplements/src/industrial_core/simple_message/src/socket/udp_socket.cpp b/training/supplements/src/industrial_core/simple_message/src/socket/udp_socket.cpp deleted file mode 100644 index 33207353..00000000 --- a/training/supplements/src/industrial_core/simple_message/src/socket/udp_socket.cpp +++ /dev/null @@ -1,146 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2011, Yaskawa America, Inc. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in the - * documentation and/or other materials provided with the distribution. - * * Neither the name of the Yaskawa America, Inc., nor the names - * of its contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef FLATHEADERS -#include "simple_message/socket/udp_socket.h" -#include "simple_message/log_wrapper.h" -#include "simple_message/simple_message.h" -#else -#include "udp_socket.h" -#include "log_wrapper.h" -#include "simple_message.h" -#endif - - -using namespace industrial::smpl_msg_connection; -using namespace industrial::byte_array; -using namespace industrial::simple_message; -using namespace industrial::shared_types; - -namespace industrial -{ -namespace udp_socket -{ - -UdpSocket::UdpSocket() -// Constructor for UDP socket object -{ - this->setSockHandle(this->SOCKET_FAIL); - memset(&this->sockaddr_, 0, sizeof(this->sockaddr_)); - -} - -UdpSocket::~UdpSocket() -// Destructor for UDP socket object -// Closes socket -{ - CLOSE(this->getSockHandle()); -} - -bool UdpSocket::receiveMsg(SimpleMessage & message) -{ - ByteArray msgBuffer; - - bool rtn = false; - shared_int size = 0; - - rtn = this->receiveBytes(msgBuffer, 0); - - if (rtn) - { - LOG_DEBUG("Receive message bytes: %u", msgBuffer.getBufferSize()); - if (msgBuffer.getBufferSize() >= sizeof(shared_int)) - { - LOG_DEBUG("Unloading message length from front of the buffer"); - msgBuffer.unloadFront((void*)(&size), sizeof(shared_int)); - - if ( size != (shared_int) msgBuffer.getBufferSize() ) - { - LOG_WARN("readBytes returned a message other than the expected size"); - } - rtn = message.init(msgBuffer); - - if (rtn) - { - rtn = true; - } - else - { - LOG_ERROR("Failed to initialize message"); - rtn = false; - } - } - else - { - LOG_ERROR("Receive bytes returned small: %d message", rtn); - LOG_ERROR("Possible handshake or other connection issue, setting disconnected"); - this->setConnected(false); - rtn = false; - } - - } - else - { - LOG_ERROR("Failed to receive message"); - rtn = false; - } - - return rtn; -} - -int UdpSocket::rawSendBytes(char *buffer, shared_int num_bytes) -{ - int rc = this->SOCKET_FAIL; - - rc = SEND_TO(this->getSockHandle(), buffer, - num_bytes, 0, (sockaddr *)&this->sockaddr_, - sizeof(this->sockaddr_)); - - return rc; -} - -int UdpSocket::rawReceiveBytes(char *buffer, shared_int num_bytes) -{ - int rc = this->SOCKET_FAIL; - SOCKLEN_T addrSize = 0; - - addrSize = sizeof(this->sockaddr_); - - rc = RECV_FROM(this->getSockHandle(), &this->buffer_[0], this->MAX_BUFFER_SIZE, - 0, (sockaddr *)&this->sockaddr_, &addrSize); - - return rc; -} - - -} //udp_socket -} //industrial - diff --git a/training/supplements/src/industrial_core/simple_message/test/utest.cpp b/training/supplements/src/industrial_core/simple_message/test/utest.cpp deleted file mode 100644 index 2ef1c3ce..00000000 --- a/training/supplements/src/industrial_core/simple_message/test/utest.cpp +++ /dev/null @@ -1,550 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2011, Southwest Research Institute - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in the - * documentation and/or other materials provided with the distribution. - * * Neither the name of the Southwest Research Institute, nor the names - * of its contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#include "simple_message/simple_message.h" -#include "simple_message/byte_array.h" -#include "simple_message/shared_types.h" -#include "simple_message/smpl_msg_connection.h" -#include "simple_message/socket/udp_client.h" -#include "simple_message/socket/udp_server.h" -#include "simple_message/socket/tcp_client.h" -#include "simple_message/socket/tcp_server.h" -#include "simple_message/ping_message.h" -#include "simple_message/ping_handler.h" -#include "simple_message/messages/joint_message.h" -#include "simple_message/joint_data.h" -#include "simple_message/message_manager.h" -#include "simple_message/simple_comms_fault_handler.h" -#include "simple_message/joint_traj_pt.h" -#include "simple_message/messages/joint_traj_pt_message.h" -#include "simple_message/typed_message.h" -#include "simple_message/joint_traj.h" - -#include -// Use pthread instead of boost::thread so we can cancel the TCP/UDP server -// threads -//#include -#include - -using namespace industrial::simple_message; -using namespace industrial::byte_array; -using namespace industrial::shared_types; -using namespace industrial::smpl_msg_connection; -using namespace industrial::udp_socket; -using namespace industrial::udp_client; -using namespace industrial::udp_server; -using namespace industrial::tcp_socket; -using namespace industrial::tcp_client; -using namespace industrial::tcp_server; -using namespace industrial::ping_message; -using namespace industrial::ping_handler; -using namespace industrial::joint_data; -using namespace industrial::joint_message; -using namespace industrial::message_manager; -using namespace industrial::simple_comms_fault_handler; -using namespace industrial::joint_traj_pt; -using namespace industrial::joint_traj_pt_message; -using namespace industrial::typed_message; -using namespace industrial::joint_traj; - -// Multiple tests require TEST_PORT_BASE to be defined. This is defined -// by the make file at compile time. -//#define TEST_PORT_BASE 11000 - -TEST(ByteArraySuite, init) -{ - - const shared_int SIZE = 100; - - ByteArray bytes; - shared_int TOO_BIG = bytes.getMaxBufferSize()+1; - - char bigBuffer[TOO_BIG]; - char buffer[SIZE]; - - // Valid byte arrays - EXPECT_TRUE(bytes.init(&buffer[0], SIZE)); - EXPECT_EQ((shared_int)bytes.getBufferSize(), SIZE); - - // Invalid init (too big) - // Invalid buffers - EXPECT_FALSE(bytes.init(&bigBuffer[0], TOO_BIG)); -} - -TEST(ByteArraySuite, loading) -{ - const shared_int SIZE = 100; - char buffer[SIZE]; - - ByteArray bytes; - ByteArray empty; - - ASSERT_TRUE(bytes.init(&buffer[0], SIZE)); - - shared_bool bIN = true, bOUT = false; - shared_int iIN = 999, iOUT = 0; - shared_real rIN = 9999.9999, rOUT = 0; - - // Boolean loading - EXPECT_TRUE(bytes.load(bIN)); - EXPECT_EQ(bytes.getBufferSize(), SIZE+sizeof(shared_bool)); - EXPECT_TRUE(bytes.unload(bOUT)); - EXPECT_EQ((shared_int)bytes.getBufferSize(), SIZE); - EXPECT_EQ(bOUT, bIN); - - // Integer loading - EXPECT_TRUE(bytes.load(iIN)); - EXPECT_EQ(bytes.getBufferSize(), SIZE+sizeof(shared_int)); - EXPECT_TRUE(bytes.unload(iOUT)); - EXPECT_EQ((shared_int)bytes.getBufferSize(), SIZE); - EXPECT_EQ(iOUT, iIN); - - // Real loading - EXPECT_TRUE(bytes.load(rIN)); - EXPECT_EQ(bytes.getBufferSize(), SIZE+sizeof(shared_real)); - EXPECT_TRUE(bytes.unload(rOUT)); - EXPECT_EQ((shared_int)bytes.getBufferSize(), SIZE); - EXPECT_EQ(rOUT, rIN); - - // Unloading a single member (down to an empty buffer size) - EXPECT_TRUE(empty.load(bIN)); - EXPECT_EQ(empty.getBufferSize(), sizeof(shared_bool)); - EXPECT_TRUE(empty.unload(bOUT)); - EXPECT_EQ((int)empty.getBufferSize(), 0); - EXPECT_EQ(bOUT, bIN); - - // Loading two members (unloading the first) and then checking the value of the second - rOUT = 0.0; - iOUT = 0; - EXPECT_TRUE(empty.load(rIN)); - EXPECT_EQ(empty.getBufferSize(), sizeof(shared_real)); - EXPECT_TRUE(empty.load(iIN)); - EXPECT_EQ(empty.getBufferSize(), sizeof(shared_real)+sizeof(shared_int)); - EXPECT_TRUE(empty.unloadFront(rOUT)); - EXPECT_EQ(rOUT, rIN); - EXPECT_TRUE(empty.unload(iOUT)); - EXPECT_EQ((int)empty.getBufferSize(), 0); - EXPECT_EQ(iOUT, iIN); -} - -TEST(ByteArraySuite, byteSwapping) -{ - if(ByteArray::isByteSwapEnabled()) - { - ASSERT_TRUE(ByteArray::isByteSwapEnabled()); - - ByteArray swapped; - unsigned char buffer[] = { - 0x00, 0x00, 0x00, 0x38, // be: 56 - 0x00, 0x00, 0x00, 0x0a, // be: 10 - 0x00, 0x00, 0x00, 0x01, // be: 1 - - 0x3e, 0x81, 0x32, 0x64, // be: 0.25233757495880127 - 0x3f, 0x30, 0x4b, 0x75, // be: 0.68865138292312622 - 0x3f, 0xa8, 0x9d, 0xd2, // be: 1.3173162937164307 - 0x3f, 0x85, 0x93, 0xdd, // be: 1.0435749292373657 - 0xbf, 0xf4, 0x8c, 0xc5, // be: -1.9105459451675415 - - }; - const unsigned int bufferLength = 32; - shared_int tempInt; - shared_real tempReal; - - swapped.init((const char*) buffer, bufferLength); - ASSERT_EQ(swapped.getBufferSize(), bufferLength); - - ASSERT_TRUE(swapped.unload(tempReal)); - EXPECT_FLOAT_EQ(tempReal, -1.9105459451675415); - - ASSERT_TRUE(swapped.unload(tempReal)); - EXPECT_FLOAT_EQ(tempReal, 1.0435749292373657); - - ASSERT_TRUE(swapped.unload(tempReal)); - EXPECT_FLOAT_EQ(tempReal, 1.3173162937164307); - - ASSERT_TRUE(swapped.unload(tempReal)); - EXPECT_FLOAT_EQ(tempReal, 0.68865138292312622); - - ASSERT_TRUE(swapped.unload(tempReal)); - EXPECT_FLOAT_EQ(tempReal, 0.25233757495880127); - - ASSERT_TRUE(swapped.unload(tempInt)); - EXPECT_EQ(tempInt, 1); - - ASSERT_TRUE(swapped.unload(tempInt)); - EXPECT_EQ(tempInt, 10); - - ASSERT_TRUE(swapped.unload(tempInt)); - EXPECT_EQ(tempInt, 56); - - ASSERT_EQ(swapped.getBufferSize(), 0); - } - -} - -TEST(ByteArraySuite, copy) -{ - - const shared_int SIZE = 100; - char buffer[SIZE]; - - // Copy - ByteArray copyFrom; - ByteArray copyTo; - ByteArray tooBig; - - - shared_int TOO_BIG = tooBig.getMaxBufferSize()-1; - char bigBuffer[TOO_BIG]; - - EXPECT_TRUE(copyFrom.init(&buffer[0], SIZE)); - EXPECT_TRUE(copyTo.load(copyFrom)); - EXPECT_EQ((shared_int)copyTo.getBufferSize(), SIZE); - EXPECT_TRUE(copyTo.load(copyFrom)); - EXPECT_EQ((shared_int)copyTo.getBufferSize(), 2*SIZE); - - // Copy too large - EXPECT_TRUE(tooBig.init(&bigBuffer[0], TOO_BIG)); - EXPECT_FALSE(copyTo.load(tooBig)); - // A failed load should not change the buffer. - EXPECT_EQ((shared_int)copyTo.getBufferSize(), 2*SIZE); -} - -// Need access to protected members for testing -class TestTcpClient : public TcpClient -{ - public: - bool sendBytes(ByteArray & buffer) - { - return TcpClient::sendBytes(buffer); - }; -}; -class TestTcpServer : public TcpServer -{ - public: - bool receiveBytes(ByteArray & buffer, shared_int num_bytes) - { - return TcpServer::receiveBytes(buffer, num_bytes); - } -}; -TEST(SocketSuite, read) -{ - const int tcpPort = TEST_PORT_BASE; - char ipAddr[] = "127.0.0.1"; - - TestTcpClient tcpClient; - TestTcpServer tcpServer; - ByteArray send, recv; - shared_int DATA = 99; - shared_int TWO_INTS = 2 * sizeof(shared_int); - shared_int ONE_INTS = 1 * sizeof(shared_int); - - // Construct server - ASSERT_TRUE(tcpServer.init(tcpPort)); - - // Construct a client - ASSERT_TRUE(tcpClient.init(&ipAddr[0], tcpPort)); - ASSERT_TRUE(tcpClient.makeConnect()); - - ASSERT_TRUE(tcpServer.makeConnect()); - - ASSERT_TRUE(send.load(DATA)); - - // Send just right amount - ASSERT_TRUE(tcpClient.sendBytes(send)); - ASSERT_TRUE(tcpClient.sendBytes(send)); - ASSERT_TRUE(tcpServer.receiveBytes(recv, TWO_INTS)); - ASSERT_EQ(TWO_INTS, recv.getBufferSize()); - - - // Send too many bytes - ASSERT_TRUE(tcpClient.sendBytes(send)); - ASSERT_TRUE(tcpClient.sendBytes(send)); - ASSERT_TRUE(tcpClient.sendBytes(send)); - ASSERT_TRUE(tcpServer.receiveBytes(recv, TWO_INTS)); - ASSERT_EQ(TWO_INTS, recv.getBufferSize()); - ASSERT_TRUE(tcpServer.receiveBytes(recv, ONE_INTS)); - ASSERT_EQ(ONE_INTS, recv.getBufferSize()); -} - - -// Utility for running tcp client in sending loop -void* -spinSender(void* arg) -{ - TestTcpClient* client = (TestTcpClient*)arg; - ByteArray send; - const int DATA = 256; - - send.load(DATA); - - while(true) - { - client->sendBytes(send); - sleep(2); - } -} - -TEST(SocketSuite, splitPackets) -{ - const int tcpPort = TEST_PORT_BASE + 1; - char ipAddr[] = "127.0.0.1"; - const int RECV_LENGTH = 64; - - TestTcpClient tcpClient; - TestTcpServer tcpServer; - ByteArray recv; -// Construct server - ASSERT_TRUE(tcpServer.init(tcpPort)); - - // Construct a client - ASSERT_TRUE(tcpClient.init(&ipAddr[0], tcpPort)); - ASSERT_TRUE(tcpClient.makeConnect()); - - ASSERT_TRUE(tcpServer.makeConnect()); - - pthread_t senderThrd; - pthread_create(&senderThrd, NULL, spinSender, &tcpClient); - - ASSERT_TRUE(tcpServer.receiveBytes(recv, RECV_LENGTH)); - ASSERT_EQ(RECV_LENGTH, recv.getBufferSize()); - - pthread_cancel(senderThrd); - pthread_join(senderThrd, NULL); -} - - -TEST(SimpleMessageSuite, init) -{ - SimpleMessage msg; - ByteArray bytes; - - // Valid messages - EXPECT_TRUE(msg.init(StandardMsgTypes::PING, CommTypes::TOPIC, ReplyTypes::INVALID, bytes)); - EXPECT_TRUE(msg.init(StandardMsgTypes::PING, CommTypes::SERVICE_REQUEST,ReplyTypes::INVALID, bytes)); - EXPECT_TRUE(msg.init(StandardMsgTypes::PING, CommTypes::SERVICE_REPLY,ReplyTypes::SUCCESS, bytes)); - EXPECT_TRUE(msg.init(StandardMsgTypes::PING, CommTypes::SERVICE_REPLY,ReplyTypes::FAILURE, bytes)); - - // Unused command - EXPECT_FALSE(msg.init(StandardMsgTypes::INVALID, CommTypes::INVALID,ReplyTypes::INVALID, bytes)); - - // Service request with a reply - EXPECT_FALSE(msg.init(StandardMsgTypes::PING, CommTypes::SERVICE_REQUEST,ReplyTypes::SUCCESS, bytes)); - EXPECT_FALSE(msg.init(StandardMsgTypes::PING, CommTypes::SERVICE_REQUEST,ReplyTypes::FAILURE, bytes)); -} - -TEST(PingMessageSuite, init) -{ - PingMessage ping; - SimpleMessage msg; - - EXPECT_FALSE(ping.init(msg)); - ping.init(); - EXPECT_EQ(StandardMsgTypes::PING, ping.getMessageType()); - - ping = PingMessage(); - ASSERT_TRUE(msg.init(StandardMsgTypes::PING, CommTypes::SERVICE_REQUEST, ReplyTypes::INVALID)); - EXPECT_TRUE(ping.init(msg)); - EXPECT_EQ(StandardMsgTypes::PING, ping.getMessageType()); -} - -TEST(PingMessageSuite, toMessage) -{ - PingMessage ping; - SimpleMessage msg; - - ping.init(); - - ASSERT_TRUE(ping.toReply(msg, ReplyTypes::SUCCESS)); - EXPECT_EQ(StandardMsgTypes::PING, msg.getMessageType()); - EXPECT_EQ(CommTypes::SERVICE_REPLY, msg.getCommType()); - EXPECT_EQ(ReplyTypes::SUCCESS, msg.getReplyCode()); - - ASSERT_TRUE(ping.toRequest(msg)); - EXPECT_EQ(StandardMsgTypes::PING, msg.getMessageType()); - EXPECT_EQ(CommTypes::SERVICE_REQUEST, msg.getCommType()); - EXPECT_EQ(ReplyTypes::INVALID, msg.getReplyCode()); - - EXPECT_FALSE(ping.toTopic(msg)); - -} - -TEST(PingHandlerSuite, init) -{ - PingHandler handler; - UdpClient udp; - - ASSERT_TRUE(handler.init(&udp)); - EXPECT_EQ(StandardMsgTypes::PING, handler.getMsgType()); - - EXPECT_FALSE(handler.init(NULL)); - -} - -TEST(MessageManagerSuite, init) -{ - MessageManager manager; - UdpClient udp; - - EXPECT_TRUE(manager.init(&udp)); - EXPECT_FALSE(manager.init(NULL)); - -} - -TEST(MessageManagerSuite, addHandler) -{ - MessageManager manager; - UdpClient udp; - PingHandler handler; - - EXPECT_EQ(0, (int)manager.getNumHandlers()); - - ASSERT_TRUE(manager.init(&udp)); - EXPECT_EQ(1, (int)manager.getNumHandlers()); - EXPECT_FALSE(manager.add(NULL)); - - ASSERT_TRUE(handler.init(&udp)); - EXPECT_FALSE(manager.add(&handler)); -} - -// wrapper around MessageManager::spin() that can be passed to -// pthread_create() -void* -spinFunc(void* arg) -{ - MessageManager* mgr = (MessageManager*)arg; - mgr->spin(); - return NULL; -} - -TEST(DISABLED_MessageManagerSuite, udp) -{ - const int udpPort = TEST_PORT_BASE + 100; - char ipAddr[] = "127.0.0.1"; - - UdpClient* udpClient = new UdpClient(); - UdpServer udpServer; - SimpleMessage pingRequest, pingReply; - MessageManager udpManager; - - ASSERT_TRUE(pingRequest.init(StandardMsgTypes::PING, CommTypes::SERVICE_REQUEST, ReplyTypes::INVALID)); - - // UDP Socket testing - // Construct server and start in a thread - ASSERT_TRUE(udpServer.init(udpPort)); - ASSERT_TRUE(udpManager.init(&udpServer)); - //boost::thread udpSrvThrd(boost::bind(&MessageManager::spin, &udpManager)); - pthread_t udpSrvThrd; - pthread_create(&udpSrvThrd, NULL, spinFunc, &udpManager); - - // Construct a client and try to ping the server - ASSERT_TRUE(udpClient->init(&ipAddr[0], udpPort)); - ASSERT_TRUE(udpClient->makeConnect()); - ASSERT_TRUE(udpClient->sendMsg(pingRequest)); - ASSERT_TRUE(udpClient->receiveMsg(pingReply)); - ASSERT_TRUE(udpClient->sendAndReceiveMsg(pingRequest, pingReply)); - - // Delete client and try to reconnect - delete udpClient; - udpClient = new UdpClient(); - ASSERT_TRUE(udpClient->init(&ipAddr[0], udpPort)); - ASSERT_TRUE(udpClient->makeConnect()); - ASSERT_TRUE(udpClient->sendAndReceiveMsg(pingRequest, pingReply)); - - pthread_cancel(udpSrvThrd); - pthread_join(udpSrvThrd, NULL); -} - -TEST(DISABLED_MessageManagerSuite, tcp) -{ - const int tcpPort = TEST_PORT_BASE + 101; - char ipAddr[] = "127.0.0.1"; - - TcpClient* tcpClient = new TcpClient(); - TcpServer tcpServer; - SimpleMessage pingRequest, pingReply; - MessageManager tcpManager; - - // MessageManager uses ros::ok, which needs ros spinner - ros::AsyncSpinner spinner(0); - spinner.start(); - - ASSERT_TRUE(pingRequest.init(StandardMsgTypes::PING, CommTypes::SERVICE_REQUEST, ReplyTypes::INVALID)); - - // TCP Socket testing - - // Construct server - ASSERT_TRUE(tcpServer.init(tcpPort)); - - // Construct a client - ASSERT_TRUE(tcpClient->init(&ipAddr[0], tcpPort)); - ASSERT_TRUE(tcpClient->makeConnect()); - - // Listen for client connection, init manager and start thread - ASSERT_TRUE(tcpServer.makeConnect()); - ASSERT_TRUE(tcpManager.init(&tcpServer)); - - // TODO: The message manager is not thread safe (threads are used for testing, - // but running the message manager in a thread results in errors when the - // underlying connection is deconstructed before the manager - //boost::thread tcpSrvThrd(boost::bind(&MessageManager::spin, &tcpManager)); - pthread_t tcpSrvThrd; - pthread_create(&tcpSrvThrd, NULL, spinFunc, &tcpManager); - - // Ping the server - ASSERT_TRUE(tcpClient->sendMsg(pingRequest)); - ASSERT_TRUE(tcpClient->receiveMsg(pingReply)); - ASSERT_TRUE(tcpClient->sendAndReceiveMsg(pingRequest, pingReply)); - - // Delete client and try to reconnect - - delete tcpClient; - sleep(10); //Allow time for client to destruct and free up port - tcpClient = new TcpClient(); - - ASSERT_TRUE(tcpClient->init(&ipAddr[0], tcpPort)); - ASSERT_TRUE(tcpClient->makeConnect()); - ASSERT_TRUE(tcpClient->sendAndReceiveMsg(pingRequest, pingReply)); - - pthread_cancel(tcpSrvThrd); - pthread_join(tcpSrvThrd, NULL); - - delete tcpClient; -} - -// Run all the tests that were declared with TEST() -int main(int argc, char **argv) -{ - ros::init(argc, argv, "test"); // some tests need ROS framework - testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); -} - diff --git a/training/supplements/src/industrial_core/simple_message/test/utest_message.cpp b/training/supplements/src/industrial_core/simple_message/test/utest_message.cpp deleted file mode 100644 index b596ec23..00000000 --- a/training/supplements/src/industrial_core/simple_message/test/utest_message.cpp +++ /dev/null @@ -1,375 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2012, Southwest Research Institute - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in the - * documentation and/or other materials provided with the distribution. - * * Neither the name of the Southwest Research Institute, nor the names - * of its contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#include "simple_message/simple_message.h" -#include "simple_message/socket/tcp_client.h" -#include "simple_message/socket/tcp_server.h" -#include "simple_message/messages/joint_message.h" -#include "simple_message/joint_data.h" -#include "simple_message/joint_traj_pt.h" -#include "simple_message/messages/joint_traj_pt_message.h" -#include "simple_message/typed_message.h" -#include "simple_message/joint_traj.h" -#include "simple_message/robot_status.h" -#include "simple_message/messages/robot_status_message.h" - -#include - -using namespace industrial::simple_message; -using namespace industrial::tcp_socket; -using namespace industrial::tcp_client; -using namespace industrial::tcp_server; -using namespace industrial::joint_data; -using namespace industrial::joint_message; -using namespace industrial::joint_traj_pt; -using namespace industrial::joint_traj_pt_message; -using namespace industrial::typed_message; -using namespace industrial::joint_traj; -using namespace industrial::robot_status; -using namespace industrial::robot_status_message; - -// Message passing routine, used to send and receive a typed message -// Useful for checking the packing and unpacking of message data. -void messagePassing(TypedMessage &send, TypedMessage &recv) -{ - const int tcpPort = 11010; - char ipAddr[] = "127.0.0.1"; - - TcpClient tcpClient; - TcpServer tcpServer; - SimpleMessage msgSend, msgRecv; - - ASSERT_TRUE(send.toTopic(msgSend)); - - // Construct server - - ASSERT_TRUE(tcpServer.init(tcpPort)); - - // Construct a client - ASSERT_TRUE(tcpClient.init(&ipAddr[0], tcpPort)); - ASSERT_TRUE(tcpClient.makeConnect()); - - ASSERT_TRUE(tcpServer.makeConnect()); - - ASSERT_TRUE(tcpClient.sendMsg(msgSend)); - ASSERT_TRUE(tcpServer.receiveMsg(msgRecv)); - ASSERT_TRUE(recv.init(msgRecv)); -} - -TEST(JointMessage, init) -{ - JointData joint; - - joint.init(); - EXPECT_TRUE(joint.setJoint(0, 1.0)); - EXPECT_TRUE(joint.setJoint(1, 2.0)); - EXPECT_TRUE(joint.setJoint(2, 3.0)); - EXPECT_TRUE(joint.setJoint(3, 4.0)); - EXPECT_TRUE(joint.setJoint(4, 5.0)); - EXPECT_TRUE(joint.setJoint(5, 6.0)); - EXPECT_TRUE(joint.setJoint(6, 7.0)); - EXPECT_TRUE(joint.setJoint(7, 8.0)); - EXPECT_TRUE(joint.setJoint(8, 9.0)); - EXPECT_TRUE(joint.setJoint(9, 10.0)); - - EXPECT_FALSE(joint.setJoint(10, 11.0)); - -} - -TEST(JointMessage, equal) -{ - JointData jointlhs, jointrhs; - - jointrhs.init(); - jointlhs.init(); - jointlhs.setJoint(0, -1.0); - jointlhs.setJoint(9, 1.0); - - EXPECT_FALSE(jointlhs==jointrhs); - - jointrhs.setJoint(0, -1.0); - jointrhs.setJoint(9, 1.0); - - EXPECT_TRUE(jointlhs==jointrhs); - -} - -TEST(JointMessage, toMessage) -{ - JointData toMessage, fromMessage; - JointMessage msg; - - toMessage.init(); - toMessage.setJoint(4, 44.44); - - msg.init(1, toMessage); - - fromMessage.copyFrom(msg.getJoints()); - - EXPECT_TRUE(toMessage==fromMessage); - -} - -TEST(JointMessage, Comms) -{ - - JointMessage jointSend, jointRecv; - JointData posSend, posRecv; - - posSend.init(); - posSend.setJoint(0, 1.0); - posSend.setJoint(1, 2.0); - posSend.setJoint(2, 3.0); - posSend.setJoint(3, 4.0); - posSend.setJoint(4, 5.0); - posSend.setJoint(5, 6.0); - posSend.setJoint(6, 7.0); - posSend.setJoint(7, 8.0); - posSend.setJoint(8, 9.0); - posSend.setJoint(9, 10.0); - - jointSend.init(1, posSend); - - messagePassing(jointSend, jointRecv); - - posRecv.copyFrom(jointRecv.getJoints()); - ASSERT_TRUE(posRecv==posSend); -} - -TEST(JointTrajPt, equal) -{ - JointTrajPt lhs, rhs; - JointData joint; - - joint.init(); - ASSERT_TRUE(joint.setJoint(0, 1.0)); - ASSERT_TRUE(joint.setJoint(1, 2.0)); - ASSERT_TRUE(joint.setJoint(2, 3.0)); - ASSERT_TRUE(joint.setJoint(3, 4.0)); - ASSERT_TRUE(joint.setJoint(4, 5.0)); - ASSERT_TRUE(joint.setJoint(5, 6.0)); - ASSERT_TRUE(joint.setJoint(6, 7.0)); - ASSERT_TRUE(joint.setJoint(7, 8.0)); - ASSERT_TRUE(joint.setJoint(8, 9.0)); - ASSERT_TRUE(joint.setJoint(9, 10.0)); - - rhs.init(1.0, joint, 50.0, 100); - EXPECT_FALSE(lhs==rhs); - - lhs.init(0, joint, 0, 0); - EXPECT_FALSE(lhs==rhs); - - lhs.copyFrom(rhs); - EXPECT_TRUE(lhs==rhs); - -} - -TEST(JointTrajPt, toMessage) -{ - JointTrajPt toMessage, fromMessage; - JointTrajPtMessage msg; - - toMessage.init(); - toMessage.setSequence(99); - msg.init(toMessage); - - fromMessage.copyFrom(msg.point_); - - EXPECT_TRUE(toMessage==fromMessage); - -} - -TEST(JointTrajPt, Comms) -{ - - JointTrajPtMessage jointSend, jointRecv; - JointData data; - JointTrajPt posSend, posRecv; - - data.init(); - data.setJoint(0, 1.0); - data.setJoint(1, 2.0); - data.setJoint(2, 3.0); - data.setJoint(3, 4.0); - data.setJoint(4, 5.0); - data.setJoint(5, 6.0); - data.setJoint(6, 7.0); - data.setJoint(7, 8.0); - data.setJoint(8, 9.0); - data.setJoint(9, 10.0); - posSend.init(1, data, 99, 100); - - jointSend.init(posSend); - - messagePassing(jointSend, jointRecv); - - posRecv.copyFrom(jointRecv.point_); - ASSERT_TRUE(posRecv==posSend); -} - -TEST(JointTraj, equal) -{ - JointTraj lhs, rhs; - JointData joint; - JointTrajPt point; - - joint.init(); - ASSERT_TRUE(joint.setJoint(0, 1.0)); - ASSERT_TRUE(joint.setJoint(1, 2.0)); - ASSERT_TRUE(joint.setJoint(2, 3.0)); - ASSERT_TRUE(joint.setJoint(3, 4.0)); - ASSERT_TRUE(joint.setJoint(4, 5.0)); - ASSERT_TRUE(joint.setJoint(5, 6.0)); - ASSERT_TRUE(joint.setJoint(6, 7.0)); - ASSERT_TRUE(joint.setJoint(7, 8.0)); - ASSERT_TRUE(joint.setJoint(8, 9.0)); - ASSERT_TRUE(joint.setJoint(9, 10.0)); - - point.init(1.0, joint, 50.0, 100); - rhs.addPoint(point); - EXPECT_FALSE(lhs==rhs); - - lhs.addPoint(point); - EXPECT_TRUE(lhs==rhs); - - lhs.addPoint(point); - EXPECT_FALSE(lhs==rhs); - - lhs.copyFrom(rhs); - EXPECT_TRUE(lhs==rhs); - -} - -TEST(RobotStatus, enumerations) -{ - // Verifying the disabled state and aliases match - EXPECT_EQ(TriStates::TS_DISABLED, TriStates::TS_FALSE); - EXPECT_EQ(TriStates::TS_DISABLED, TriStates::TS_LOW); - EXPECT_EQ(TriStates::TS_DISABLED, TriStates::TS_OFF); - - // Verifying the enabled state and aliases values match - EXPECT_EQ(TriStates::TS_ENABLED, TriStates::TS_TRUE); - EXPECT_EQ(TriStates::TS_ENABLED, TriStates::TS_HIGH); - EXPECT_EQ(TriStates::TS_ENABLED, TriStates::TS_ON); - - // Verifying the unknown values match (this isn't reqd, but makes sense) - EXPECT_EQ(TriStates::TS_UNKNOWN, RobotModes::UNKNOWN); -} - -TEST(RobotStatus, init) -{ - RobotStatus status; - RobotStatus empty; - status.init(); - // An empty (non-initted) status should be initialized in the constructor. - EXPECT_TRUE(status==empty); - EXPECT_EQ(status.getDrivesPowered(), TriStates::TS_UNKNOWN); - EXPECT_EQ(status.getEStopped(), TriStates::TS_UNKNOWN); - EXPECT_EQ(status.getErrorCode(), 0); - EXPECT_EQ(status.getInError(), TriStates::TS_UNKNOWN); - EXPECT_EQ(status.getInMotion(), TriStates::TS_UNKNOWN); - EXPECT_EQ(status.getMode(), RobotModes::UNKNOWN); - EXPECT_EQ(status.getMotionPossible(), TriStates::TS_UNKNOWN); -} - -TEST(RobotStatus, equal) -{ - RobotStatus lhs, rhs; - - EXPECT_TRUE(lhs==rhs); - lhs.setDrivesPowered(TriStates::TS_ENABLED); - EXPECT_FALSE(lhs==rhs); - rhs.setDrivesPowered(TriStates::TS_ENABLED); - EXPECT_TRUE(lhs==rhs); - - lhs.setEStopped(TriStates::TS_ENABLED); - EXPECT_FALSE(lhs==rhs); - rhs.setEStopped(TriStates::TS_ENABLED); - EXPECT_TRUE(lhs==rhs); - - lhs.setErrorCode(99); - EXPECT_FALSE(lhs==rhs); - rhs.setErrorCode(99); - EXPECT_TRUE(lhs==rhs); - - lhs.setInError(TriStates::TS_ENABLED); - EXPECT_FALSE(lhs==rhs); - rhs.setInError(TriStates::TS_ENABLED); - EXPECT_TRUE(lhs==rhs); - - lhs.setInMotion(TriStates::TS_ENABLED); - EXPECT_FALSE(lhs==rhs); - rhs.setInMotion(TriStates::TS_ENABLED); - EXPECT_TRUE(lhs==rhs); - - lhs.setMode(RobotModes::AUTO); - EXPECT_FALSE(lhs==rhs); - rhs.setMode(RobotModes::AUTO); - EXPECT_TRUE(lhs==rhs); - - lhs.setMotionPossible(TriStates::TS_ENABLED); - EXPECT_FALSE(lhs==rhs); - rhs.setMotionPossible(TriStates::TS_ENABLED); - EXPECT_TRUE(lhs==rhs); - -} - -TEST(RobotStatus, toMessage) -{ - RobotStatus toMessage, fromMessage; - RobotStatusMessage msg; - - toMessage.init(TriStates::TS_ENABLED, TriStates::TS_FALSE, 99, TriStates::TS_TRUE, TriStates::TS_TRUE, RobotModes::MANUAL, - TriStates::TS_DISABLED); - msg.init(toMessage); - - fromMessage.copyFrom(msg.status_); - - EXPECT_TRUE(toMessage==fromMessage); - -} - -TEST(RobotStatus, Comms) -{ - RobotStatusMessage statusMsgSend, statusMsgRecv; - RobotStatus statusSend, statusRecv; - - statusSend.init(TriStates::TS_ENABLED, TriStates::TS_FALSE, 99, TriStates::TS_TRUE, TriStates::TS_TRUE, RobotModes::MANUAL, - TriStates::TS_DISABLED); - - statusMsgSend.init(statusSend); - - messagePassing(statusMsgSend, statusMsgRecv); - - statusRecv.copyFrom(statusMsgRecv.status_); - ASSERT_TRUE(statusRecv==statusSend); -} -