--- /dev/null
+devel/
+logs/
+build/
+bin/
+lib/
+msg_gen/
+srv_gen/
+msg/*Action.msg
+msg/*ActionFeedback.msg
+msg/*ActionGoal.msg
+msg/*ActionResult.msg
+msg/*Feedback.msg
+msg/*Goal.msg
+msg/*Result.msg
+msg/_*.py
+build_isolated/
+devel_isolated/
+
+# Generated by dynamic reconfigure
+*.cfgc
+/cfg/cpp/
+/cfg/*.py
+
+# Ignore generated docs
+#*.dox
+*.wikidoc
+
+# eclipse stuff
+.project
+.cproject
+
+# qcreator stuff
+CMakeLists.txt.user
+
+srv/_*.py
+*.pcd
+*.pyc
+qtcreator-*
+*.user
+
+/planning/cfg
+/planning/docs
+/planning/src
+
+*~
+
+# Emacs
+.#*
+
+# Catkin custom files
+CATKIN_IGNORE
--- /dev/null
+Ported from ROS Fuerte version to Indigo by Walter Fetter Lages:
+
+wam_common and wam_robot stacks in ROS Fuerte were ported to meta-packages
+in ROS Indigo.
+
+wam_msgs, wam_srvs, wam_teleop and wam_node packages in ROS Fuerte were
+ported to packages in ROS Indigo.
+
--- /dev/null
+barrett-ros-pkg -- README
+Barrett Technology Inc.
+2012-05-11
+
+Barrett Technology's ROS repository is an abstraction of Libbarrett. Libbarrett
+is a real-time controls library written in C++ that runs Barrett Technology's
+products, including the WAM Arm and the BH8-280 BarrettHand.
+
+Our ROS repository requires an installed version of Libbarrett. To check out
+the latest version of Libbarrett:
+
+svn http://web.barrett.com/svn/libbarrett/tags/libbarrett-1.0.0
+
+Please view the README.txt file within Libbarrett for installation instructions.
+
+The wam_robot stack is designed to be run on a WAM PC-104 or external control
+PC.
+
+The wam_common stack is designed as the interface to communicate with the
+functionality exposed by the wam_node.
+
+The wam_sim stack is in development and should be released shortly.
+
+Please contact us with any functionality or feature requests. We appreciate
+contributions and collaborations.
+
+Libbarrett is free software: you can redistribute it and/or
+modify it under the terms of the GNU General Public License as published by the
+Free Software Foundation.
+
+Contact us at:
+support@barrett.com
+http://www.barrett.com/
++1-617-252-9000
+
+Barrett Technology
+625 Mount Auburn Street
+Cambridge, MA 02138
+USA
--- /dev/null
+cmake_minimum_required(VERSION 2.8.3)
+project(wam_common)
+find_package(catkin REQUIRED)
+catkin_metapackage()
--- /dev/null
+<?xml version="1.0"?>
+<package>
+ <name>wam_common</name>
+ <version>2.0.0</version>
+ <description>The wam_common package
+ Stack describing common messages / service / launch files for the Barrett
+ Technology WAM and BarrettHand.
+ Ported from the ROS Fuerte version to Indigo by Walter Fetter Lages.
+ </description>
+
+ <!-- One maintainer tag required, multiple allowed, one person per tag -->
+ <!-- Example: -->
+ <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
+ <maintainer email="fetter@ece.ufrgs.br">Walter Fetter Lages</maintainer>
+
+
+ <!-- One license tag required, multiple allowed, one license per tag -->
+ <!-- Commonly used license strings: -->
+ <!-- BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
+ <license>BSD</license>
+
+
+ <!-- Url tags are optional, but multiple are allowed, one per tag -->
+ <!-- Optional attribute type can be: website, bugtracker, or repository -->
+ <!-- Example: -->
+ <!-- <url type="website">http://wiki.ros.org/wam_common</url> -->
+
+
+ <!-- Author tags are optional, multiple are allowed, one per tag -->
+ <!-- Authors do not have to be maintainers, but could be -->
+ <!-- Example: -->
+ <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
+ <author>Maintained by Barrett Techonlogy Inc., Kyle Maroney</author>
+
+ <!-- The *_depend tags are used to specify dependencies -->
+ <!-- Dependencies can be catkin packages or system dependencies -->
+ <!-- Examples: -->
+ <!-- Use build_depend for packages you need at compile time: -->
+ <!-- <build_depend>message_generation</build_depend> -->
+ <!-- Use buildtool_depend for build tool packages: -->
+ <!-- <buildtool_depend>catkin</buildtool_depend> -->
+ <!-- Use run_depend for packages you need at runtime: -->
+ <!-- <run_depend>message_runtime</run_depend> -->
+ <!-- Use test_depend for packages you need only for testing: -->
+ <!-- <test_depend>gtest</test_depend> -->
+ <buildtool_depend>catkin</buildtool_depend>
+ <run_depend>wam_msgs</run_depend>
+ <run_depend>wam_srvs</run_depend>
+ <run_depend>wam_teleop</run_depend>
+
+
+ <!-- The export tag contains other, unspecified, tags -->
+ <export>
+ <!-- Other tools can request additional information be placed here -->
+ <metapackage/>
+ </export>
+</package>
--- /dev/null
+cmake_minimum_required(VERSION 2.8.3)
+project(wam_msgs)
+
+## Compile as C++11, supported in ROS Kinetic and newer
+# add_compile_options(-std=c++11)
+
+## Find catkin macros and libraries
+## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
+## is used, also find other catkin packages
+find_package(catkin REQUIRED COMPONENTS
+ message_generation
+)
+
+## System dependencies are found with CMake's conventions
+# find_package(Boost REQUIRED COMPONENTS system)
+
+
+## Uncomment this if the package has a setup.py. This macro ensures
+## modules and global scripts declared therein get installed
+## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
+# catkin_python_setup()
+
+################################################
+## Declare ROS messages, services and actions ##
+################################################
+
+## To declare and build messages, services or actions from within this
+## package, follow these steps:
+## * Let MSG_DEP_SET be the set of packages whose message types you use in
+## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
+## * In the file package.xml:
+## * add a build_depend tag for "message_generation"
+## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET
+## * If MSG_DEP_SET isn't empty the following dependency has been pulled in
+## but can be declared for certainty nonetheless:
+## * add a run_depend tag for "message_runtime"
+## * In this file (CMakeLists.txt):
+## * add "message_generation" and every package in MSG_DEP_SET to
+## find_package(catkin REQUIRED COMPONENTS ...)
+## * add "message_runtime" and every package in MSG_DEP_SET to
+## catkin_package(CATKIN_DEPENDS ...)
+## * uncomment the add_*_files sections below as needed
+## and list every .msg/.srv/.action file to be processed
+## * uncomment the generate_messages entry below
+## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
+
+## Generate messages in the 'msg' folder
+add_message_files(
+ FILES
+# Message1.msg
+# Message2.msg
+ RTCartPos.msg
+ RTCartVel.msg
+ RTJointPos.msg
+ RTJointVel.msg
+ RTOrtnPos.msg
+ RTOrtnVel.msg
+ RTPose.msg
+)
+
+## Generate services in the 'srv' folder
+# add_service_files(
+# FILES
+# Service1.srv
+# Service2.srv
+# )
+
+## Generate actions in the 'action' folder
+# add_action_files(
+# FILES
+# Action1.action
+# Action2.action
+# )
+
+## Generate added messages and services with any dependencies listed here
+generate_messages(
+ DEPENDENCIES
+# std_msgs # Or other packages containing msgs
+)
+
+################################################
+## Declare ROS dynamic reconfigure parameters ##
+################################################
+
+## To declare and build dynamic reconfigure parameters within this
+## package, follow these steps:
+## * In the file package.xml:
+## * add a build_depend and a run_depend tag for "dynamic_reconfigure"
+## * In this file (CMakeLists.txt):
+## * add "dynamic_reconfigure" to
+## find_package(catkin REQUIRED COMPONENTS ...)
+## * uncomment the "generate_dynamic_reconfigure_options" section below
+## and list every .cfg file to be processed
+
+## Generate dynamic reconfigure parameters in the 'cfg' folder
+# generate_dynamic_reconfigure_options(
+# cfg/DynReconf1.cfg
+# cfg/DynReconf2.cfg
+# )
+
+###################################
+## catkin specific configuration ##
+###################################
+## The catkin_package macro generates cmake config files for your package
+## Declare things to be passed to dependent projects
+## INCLUDE_DIRS: uncomment this if you package contains header files
+## LIBRARIES: libraries you create in this project that dependent projects also need
+## CATKIN_DEPENDS: catkin_packages dependent projects also need
+## DEPENDS: system dependencies of this project that dependent projects also need
+catkin_package(
+# INCLUDE_DIRS include
+# LIBRARIES wam_msgs
+ CATKIN_DEPENDS message_runtime
+# DEPENDS system_lib
+)
+
+###########
+## Build ##
+###########
+
+## Specify additional locations of header files
+## Your package locations should be listed before other locations
+include_directories(
+# include
+# ${catkin_INCLUDE_DIRS}
+)
+
+## Declare a C++ library
+# add_library(${PROJECT_NAME}
+# src/${PROJECT_NAME}/wam_msgs.cpp
+# )
+
+## Add cmake target dependencies of the library
+## as an example, code may need to be generated before libraries
+## either from message generation or dynamic reconfigure
+# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Declare a C++ executable
+## With catkin_make all packages are built within a single CMake context
+## The recommended prefix ensures that target names across packages don't collide
+# add_executable(${PROJECT_NAME}_node src/wam_msgs_node.cpp)
+
+## Rename C++ executable without prefix
+## The above recommended prefix causes long target names, the following renames the
+## target back to the shorter version for ease of user use
+## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
+# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "")
+
+## Add cmake target dependencies of the executable
+## same as for the library above
+# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Specify libraries to link a library or executable target against
+# target_link_libraries(${PROJECT_NAME}_node
+# ${catkin_LIBRARIES}
+# )
+
+#############
+## Install ##
+#############
+
+# all install targets should use catkin DESTINATION variables
+# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
+
+## Mark executable scripts (Python etc.) for installation
+## in contrast to setup.py, you can choose the destination
+# install(PROGRAMS
+# scripts/my_python_script
+# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark executables and/or libraries for installation
+# install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node
+# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark cpp header files for installation
+# install(DIRECTORY include/${PROJECT_NAME}/
+# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+# FILES_MATCHING PATTERN "*.h"
+# PATTERN ".svn" EXCLUDE
+# )
+
+## Mark other files for installation (e.g. launch and bag files, etc.)
+# install(FILES
+# # myfile1
+# # myfile2
+# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+# )
+
+#############
+## Testing ##
+#############
+
+## Add gtest based cpp test target and link libraries
+# catkin_add_gtest(${PROJECT_NAME}-test test/test_wam_msgs.cpp)
+# if(TARGET ${PROJECT_NAME}-test)
+# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
+# endif()
+
+## Add folders to be run by python nosetests
+# catkin_add_nosetests(test)
--- /dev/null
+float32[3] position
+float32[3] rate_limits
--- /dev/null
+float32[3] direction
+float32 magnitude
--- /dev/null
+float32[] joints
+float32[] rate_limits
--- /dev/null
+float32[] velocities
--- /dev/null
+float32[4] orientation
+float32[4] rate_limits
--- /dev/null
+float32[3] angular
+float32 magnitude
--- /dev/null
+float32[3] position
+float32[4] orientation
+float32[3] pos_rate_limits
+float32[4] ortn_rate_limits
--- /dev/null
+<?xml version="1.0"?>
+<package>
+ <name>wam_msgs</name>
+ <version>2.0.0</version>
+ <description>The wam_msgss package
+ This Package contains WAM / BarrettHand specific messages.
+ Ported from the ROS Fuerte version to Indigo by Walter Fetter Lages.
+ </description>
+
+ <!-- One maintainer tag required, multiple allowed, one person per tag -->
+ <!-- Example: -->
+ <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
+ <maintainer email="fetter@ece.ufrgs.br">Walter Fetter Lages</maintainer>
+
+
+ <!-- One license tag required, multiple allowed, one license per tag -->
+ <!-- Commonly used license strings: -->
+ <!-- BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
+ <license>BSD</license>
+
+
+ <!-- Url tags are optional, but multiple are allowed, one per tag -->
+ <!-- Optional attribute type can be: website, bugtracker, or repository -->
+ <!-- Example: -->
+ <!-- <url type="website">http://wiki.ros.org/wam_msgs</url> -->
+
+
+ <!-- Author tags are optional, multiple are allowed, one per tag -->
+ <!-- Authors do not have to be maintainers, but could be -->
+ <!-- Example: -->
+ <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
+ <author>Barrett Technology Inc., Kyle Maroney</author>
+
+
+ <!-- The *_depend tags are used to specify dependencies -->
+ <!-- Dependencies can be catkin packages or system dependencies -->
+ <!-- Examples: -->
+ <!-- Use build_depend for packages you need at compile time: -->
+ <!-- <build_depend>message_generation</build_depend> -->
+ <!-- Use buildtool_depend for build tool packages: -->
+ <!-- <buildtool_depend>catkin</buildtool_depend> -->
+ <!-- Use run_depend for packages you need at runtime: -->
+ <!-- <run_depend>message_runtime</run_depend> -->
+ <!-- Use test_depend for packages you need only for testing: -->
+ <!-- <test_depend>gtest</test_depend> -->
+ <buildtool_depend>catkin</buildtool_depend>
+ <buildtool_depend>message_generation</buildtool_depend>
+
+ <run_depend>message_runtime</run_depend>
+
+ <!-- The export tag contains other, unspecified, tags -->
+ <export>
+ <!-- Other tools can request additional information be placed here -->
+
+ </export>
+</package>
--- /dev/null
+cmake_minimum_required(VERSION 2.8.3)
+project(wam_node)
+
+## Compile as C++11, supported in ROS Kinetic and newer
+# add_compile_options(-std=c++11)
+
+## Find catkin macros and libraries
+## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
+## is used, also find other catkin packages
+find_package(catkin REQUIRED COMPONENTS
+ geometry_msgs
+ roscpp
+ sensor_msgs
+ std_msgs
+ std_srvs
+ tf
+ wam_msgs
+ wam_srvs
+)
+
+## System dependencies are found with CMake's conventions
+# find_package(Boost REQUIRED COMPONENTS system)
+
+
+## Uncomment this if the package has a setup.py. This macro ensures
+## modules and global scripts declared therein get installed
+## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
+# catkin_python_setup()
+
+################################################
+## Declare ROS messages, services and actions ##
+################################################
+
+## To declare and build messages, services or actions from within this
+## package, follow these steps:
+## * Let MSG_DEP_SET be the set of packages whose message types you use in
+## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
+## * In the file package.xml:
+## * add a build_depend tag for "message_generation"
+## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET
+## * If MSG_DEP_SET isn't empty the following dependency has been pulled in
+## but can be declared for certainty nonetheless:
+## * add a run_depend tag for "message_runtime"
+## * In this file (CMakeLists.txt):
+## * add "message_generation" and every package in MSG_DEP_SET to
+## find_package(catkin REQUIRED COMPONENTS ...)
+## * add "message_runtime" and every package in MSG_DEP_SET to
+## catkin_package(CATKIN_DEPENDS ...)
+## * uncomment the add_*_files sections below as needed
+## and list every .msg/.srv/.action file to be processed
+## * uncomment the generate_messages entry below
+## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
+
+## Generate messages in the 'msg' folder
+# add_message_files(
+# FILES
+# Message1.msg
+# Message2.msg
+# )
+
+## Generate services in the 'srv' folder
+# add_service_files(
+# FILES
+# Service1.srv
+# Service2.srv
+# )
+
+## Generate actions in the 'action' folder
+# add_action_files(
+# FILES
+# Action1.action
+# Action2.action
+# )
+
+## Generate added messages and services with any dependencies listed here
+# generate_messages(
+# DEPENDENCIES
+# geometry_msgs# sensor_msgs# std_msgs# wam_msgs
+# )
+
+################################################
+## Declare ROS dynamic reconfigure parameters ##
+################################################
+
+## To declare and build dynamic reconfigure parameters within this
+## package, follow these steps:
+## * In the file package.xml:
+## * add a build_depend and a run_depend tag for "dynamic_reconfigure"
+## * In this file (CMakeLists.txt):
+## * add "dynamic_reconfigure" to
+## find_package(catkin REQUIRED COMPONENTS ...)
+## * uncomment the "generate_dynamic_reconfigure_options" section below
+## and list every .cfg file to be processed
+
+## Generate dynamic reconfigure parameters in the 'cfg' folder
+# generate_dynamic_reconfigure_options(
+# cfg/DynReconf1.cfg
+# cfg/DynReconf2.cfg
+# )
+
+###################################
+## catkin specific configuration ##
+###################################
+## The catkin_package macro generates cmake config files for your package
+## Declare things to be passed to dependent projects
+## INCLUDE_DIRS: uncomment this if you package contains header files
+## LIBRARIES: libraries you create in this project that dependent projects also need
+## CATKIN_DEPENDS: catkin_packages dependent projects also need
+## DEPENDS: system dependencies of this project that dependent projects also need
+catkin_package(
+# INCLUDE_DIRS include
+# LIBRARIES wam_node
+# CATKIN_DEPENDS geometry_msgs roscpp sensor_msgs std_msgs std_srvs tf wam_msgs wam_srvs
+# DEPENDS system_lib
+)
+
+###########
+## Build ##
+###########
+
+## Specify additional locations of header files
+## Your package locations should be listed before other locations
+include_directories(
+# include
+ ${catkin_INCLUDE_DIRS}
+)
+
+## Declare a C++ library
+# add_library(${PROJECT_NAME}
+# src/${PROJECT_NAME}/wam_node.cpp
+# )
+
+## Add cmake target dependencies of the library
+## as an example, code may need to be generated before libraries
+## either from message generation or dynamic reconfigure
+# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Declare a C++ executable
+## With catkin_make all packages are built within a single CMake context
+## The recommended prefix ensures that target names across packages don't collide
+add_executable(${PROJECT_NAME} src/${PROJECT_NAME}.cpp)
+
+## Rename C++ executable without prefix
+## The above recommended prefix causes long target names, the following renames the
+## target back to the shorter version for ease of user use
+## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
+# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "")
+
+## Add cmake target dependencies of the executable
+## same as for the library above
+# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Specify libraries to link a library or executable target against
+target_link_libraries(${PROJECT_NAME}
+ ${catkin_LIBRARIES}
+)
+
+#############
+## Install ##
+#############
+
+# all install targets should use catkin DESTINATION variables
+# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
+
+## Mark executable scripts (Python etc.) for installation
+## in contrast to setup.py, you can choose the destination
+# install(PROGRAMS
+# scripts/my_python_script
+# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark executables and/or libraries for installation
+# install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node
+# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark cpp header files for installation
+# install(DIRECTORY include/${PROJECT_NAME}/
+# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+# FILES_MATCHING PATTERN "*.h"
+# PATTERN ".svn" EXCLUDE
+# )
+
+## Mark other files for installation (e.g. launch and bag files, etc.)
+# install(FILES
+# # myfile1
+# # myfile2
+# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+# )
+
+#############
+## Testing ##
+#############
+
+## Add gtest based cpp test target and link libraries
+# catkin_add_gtest(${PROJECT_NAME}-test test/test_wam_node.cpp)
+# if(TARGET ${PROJECT_NAME}-test)
+# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
+# endif()
+
+## Add folders to be run by python nosetests
+# catkin_add_nosetests(test)
--- /dev/null
+<launch>
+ <node name="wam_node" type="wam_node" pkg="wam_node" output="screen" />
+</launch>
+
--- /dev/null
+<?xml version="1.0"?>
+<package>
+ <name>wam_node</name>
+ <version>2.0.0</version>
+ <description>The wam_node package
+ Barrett Technology ROS node for 4-DOF and 7-DOF WAM to be run on WAM robot
+ PC or external PC via CAN.
+ Ported from the ROS Fuerte version to Indigo by Walter Fetter Lages.
+ </description>
+
+ <!-- One maintainer tag required, multiple allowed, one person per tag -->
+ <!-- Example: -->
+ <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
+ <maintainer email="fetter@ece.ufrgs.br">Walter Fetter Lages</maintainer>
+
+
+ <!-- One license tag required, multiple allowed, one license per tag -->
+ <!-- Commonly used license strings: -->
+ <!-- BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
+ <license>GPL</license>
+
+
+ <!-- Url tags are optional, but multiple are allowed, one per tag -->
+ <!-- Optional attribute type can be: website, bugtracker, or repository -->
+ <!-- Example: -->
+ <!-- <url type="website">http://wiki.ros.org/wam_node</url> -->
+ <url type="website">http://ros.org/wiki/wam_node</url>
+
+
+ <!-- Author tags are optional, multiple are allowed, one per tag -->
+ <!-- Authors do not have to be maintainers, but could be -->
+ <!-- Example: -->
+ <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
+ <author>Barrett Technology Inc., Kyle Maroney</author>
+
+
+ <!-- The *_depend tags are used to specify dependencies -->
+ <!-- Dependencies can be catkin packages or system dependencies -->
+ <!-- Examples: -->
+ <!-- Use build_depend for packages you need at compile time: -->
+ <!-- <build_depend>message_generation</build_depend> -->
+ <!-- Use buildtool_depend for build tool packages: -->
+ <!-- <buildtool_depend>catkin</buildtool_depend> -->
+ <!-- Use run_depend for packages you need at runtime: -->
+ <!-- <run_depend>message_runtime</run_depend> -->
+ <!-- Use test_depend for packages you need only for testing: -->
+ <!-- <test_depend>gtest</test_depend> -->
+ <buildtool_depend>catkin</buildtool_depend>
+ <build_depend>geometry_msgs</build_depend>
+ <build_depend>roscpp</build_depend>
+ <build_depend>sensor_msgs</build_depend>
+ <build_depend>std_msgs</build_depend>
+ <build_depend>std_srvs</build_depend>
+ <build_depend>tf</build_depend>
+ <build_depend>wam_msgs</build_depend>
+ <build_depend>wam_srvs</build_depend>
+ <run_depend>geometry_msgs</run_depend>
+ <run_depend>roscpp</run_depend>
+ <run_depend>sensor_msgs</run_depend>
+ <run_depend>std_msgs</run_depend>
+ <run_depend>std_srvs</run_depend>
+ <run_depend>tf</run_depend>
+ <run_depend>wam_msgs</run_depend>
+ <run_depend>wam_srvs</run_depend>
+
+
+ <!-- The export tag contains other, unspecified, tags -->
+ <export>
+ <!-- Other tools can request additional information be placed here -->
+
+ </export>
+</package>
--- /dev/null
+/*
+ Copyright 2012 Barrett Technology <support@barrett.com>
+
+ This file is part of barrett-ros-pkg.
+
+ This version of barrett-ros-pkg is free software: you can redistribute it
+ and/or modify it under the terms of the GNU General Public License as
+ published by the Free Software Foundation, either version 3 of the
+ License, or (at your option) any later version.
+
+ This version of barrett-ros-pkg is distributed in the hope that it will be
+ useful, but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ GNU General Public License for more details.
+
+ You should have received a copy of the GNU General Public License along
+ with this version of barrett-ros-pkg. If not, see
+ <http://www.gnu.org/licenses/>.
+
+ Barrett Technology holds all copyrights on barrett-ros-pkg. As the sole
+ copyright holder, Barrett reserves the right to release future versions
+ of barrett-ros-pkg under a different license.
+
+ File: wam_node.cpp
+ Date: 5 June, 2012
+ Author: Kyle Maroney
+ */
+
+#include <unistd.h>
+#include <math.h>
+
+#include <boost/thread.hpp> // BarrettHand threading
+#include <boost/bind.hpp>
+
+#include "ros/ros.h"
+#include "tf/transform_datatypes.h"
+
+#include "wam_msgs/RTJointPos.h"
+#include "wam_msgs/RTJointVel.h"
+#include "wam_msgs/RTCartPos.h"
+#include "wam_msgs/RTCartVel.h"
+#include "wam_msgs/RTOrtnPos.h"
+#include "wam_msgs/RTOrtnVel.h"
+#include "wam_srvs/GravityComp.h"
+#include "wam_srvs/Hold.h"
+#include "wam_srvs/JointMove.h"
+#include "wam_srvs/PoseMove.h"
+#include "wam_srvs/CartPosMove.h"
+#include "wam_srvs/OrtnMove.h"
+#include "wam_srvs/BHandFingerPos.h"
+#include "wam_srvs/BHandGraspPos.h"
+#include "wam_srvs/BHandSpreadPos.h"
+#include "wam_srvs/BHandFingerVel.h"
+#include "wam_srvs/BHandGraspVel.h"
+#include "wam_srvs/BHandSpreadVel.h"
+#include "std_srvs/Empty.h"
+#include "sensor_msgs/JointState.h"
+#include "geometry_msgs/PoseStamped.h"
+
+#include <barrett/math.h>
+#include <barrett/units.h>
+#include <barrett/systems.h>
+#include <barrett/products/product_manager.h>
+#include <barrett/standard_main_function.h>
+#include <barrett/systems/wam.h>
+#include <barrett/detail/stl_utils.h>
+
+static const int PUBLISH_FREQ = 250; // Default Control Loop / Publishing Frequency
+static const int BHAND_PUBLISH_FREQ = 5; // Publishing Frequency for the BarretHand
+static const double SPEED = 0.03; // Default Cartesian Velocity
+
+using namespace barrett;
+
+//Creating a templated multiplier for our real-time computation
+template<typename T1, typename T2, typename OutputType>
+ class Multiplier : public systems::System, public systems::SingleOutput<OutputType>
+ {
+ public:
+ Input<T1> input1;
+ public:
+ Input<T2> input2;
+
+ public:
+ Multiplier(std::string sysName = "Multiplier") :
+ systems::System(sysName), systems::SingleOutput<OutputType>(this), input1(this), input2(this)
+ {
+ }
+ virtual ~Multiplier()
+ {
+ mandatoryCleanUp();
+ }
+
+ protected:
+ OutputType data;
+ virtual void operate()
+ {
+ data = input1.getValue() * input2.getValue();
+ this->outputValue->setData(&data);
+ }
+
+ private:
+ DISALLOW_COPY_AND_ASSIGN(Multiplier);
+
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ };
+
+//Creating a templated converter from Roll, Pitch, Yaw to Quaternion for real-time computation
+class ToQuaternion : public systems::SingleIO<math::Vector<3>::type, Eigen::Quaterniond>
+{
+public:
+ Eigen::Quaterniond outputQuat;
+
+public:
+ ToQuaternion(std::string sysName = "ToQuaternion") :
+ systems::SingleIO<math::Vector<3>::type, Eigen::Quaterniond>(sysName)
+ {
+ }
+ virtual ~ToQuaternion()
+ {
+ mandatoryCleanUp();
+ }
+
+protected:
+ btQuaternion q;
+ virtual void operate()
+ {
+ const math::Vector<3>::type &inputRPY = input.getValue();
+ q.setEulerZYX(inputRPY[2], inputRPY[1], inputRPY[0]);
+ outputQuat.x() = q.getX();
+ outputQuat.y() = q.getY();
+ outputQuat.z() = q.getZ();
+ outputQuat.w() = q.getW();
+ this->outputValue->setData(&outputQuat);
+ }
+
+private:
+ DISALLOW_COPY_AND_ASSIGN(ToQuaternion);
+
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+};
+
+//Simple Function for converting Quaternion to RPY
+math::Vector<3>::type toRPY(Eigen::Quaterniond inquat)
+{
+ math::Vector<3>::type newRPY;
+ btQuaternion q(inquat.x(), inquat.y(), inquat.z(), inquat.w());
+ btMatrix3x3(q).getEulerZYX(newRPY[2], newRPY[1], newRPY[0]);
+ return newRPY;
+}
+
+//WamNode Class
+template<size_t DOF>
+ class WamNode
+ {
+ BARRETT_UNITS_TEMPLATE_TYPEDEFS(DOF);
+ protected:
+ bool cart_vel_status, ortn_vel_status, jnt_vel_status;
+ bool jnt_pos_status, cart_pos_status, ortn_pos_status, new_rt_cmd;
+ double cart_vel_mag, ortn_vel_mag;
+ systems::Wam<DOF>& wam;
+ Hand* hand;
+ jp_type jp, jp_cmd, jp_home;
+ jp_type rt_jp_cmd, rt_jp_rl;
+ jv_type rt_jv_cmd;
+ cp_type cp_cmd, rt_cv_cmd;
+ cp_type rt_cp_cmd, rt_cp_rl;
+ Eigen::Quaterniond ortn_cmd, rt_op_cmd, rt_op_rl;
+ pose_type pose_cmd;
+ math::Vector<3>::type rt_ortn_cmd;
+ systems::ExposedOutput<Eigen::Quaterniond> orientationSetPoint, current_ortn;
+ systems::ExposedOutput<cp_type> cart_dir, current_cart_pos, cp_track;
+ systems::ExposedOutput<math::Vector<3>::type> rpy_cmd, current_rpy_ortn;
+ systems::ExposedOutput<jv_type> jv_track;
+ systems::ExposedOutput<jp_type> jp_track;
+ systems::TupleGrouper<cp_type, Eigen::Quaterniond> rt_pose_cmd;
+ systems::Summer<cp_type> cart_pos_sum;
+ systems::Summer<math::Vector<3>::type> ortn_cmd_sum;
+ systems::Ramp ramp;
+ systems::RateLimiter<jp_type> jp_rl;
+ systems::RateLimiter<cp_type> cp_rl;
+ Multiplier<double, cp_type, cp_type> mult_linear;
+ Multiplier<double, math::Vector<3>::type, math::Vector<3>::type> mult_angular;
+ ToQuaternion to_quat, to_quat_print;
+ Eigen::Quaterniond ortn_print;
+ ros::Time last_cart_vel_msg_time, last_ortn_vel_msg_time, last_jnt_vel_msg_time;
+ ros::Time last_jnt_pos_msg_time, last_cart_pos_msg_time, last_ortn_pos_msg_time;
+ ros::Duration rt_msg_timeout;
+
+ //Subscribed Topics
+ wam_msgs::RTCartVel cart_vel_cmd;
+ wam_msgs::RTOrtnVel ortn_vel_cmd;
+
+ //Subscribers
+ ros::Subscriber cart_vel_sub;
+ ros::Subscriber ortn_vel_sub;
+ ros::Subscriber jnt_vel_sub;
+ ros::Subscriber jnt_pos_sub;
+ ros::Subscriber cart_pos_sub;
+ ros::Subscriber ortn_pos_sub;
+
+ //Published Topics
+ sensor_msgs::JointState wam_joint_state, bhand_joint_state;
+ geometry_msgs::PoseStamped wam_pose;
+
+ //Publishers
+ ros::Publisher wam_joint_state_pub, bhand_joint_state_pub, wam_pose_pub;
+
+ //Services
+ ros::ServiceServer gravity_srv, go_home_srv, hold_jpos_srv, hold_cpos_srv;
+ ros::ServiceServer hold_ortn_srv, joint_move_srv, pose_move_srv;
+ ros::ServiceServer cart_move_srv, ortn_move_srv, hand_close_srv;
+ ros::ServiceServer hand_open_grsp_srv, hand_close_grsp_srv, hand_open_sprd_srv;
+ ros::ServiceServer hand_close_sprd_srv, hand_fngr_pos_srv, hand_fngr_vel_srv;
+ ros::ServiceServer hand_grsp_pos_srv, hand_grsp_vel_srv, hand_sprd_pos_srv;
+ ros::ServiceServer hand_sprd_vel_srv;
+
+ public:
+ WamNode(systems::Wam<DOF>& wam_) :
+ wam(wam_), hand(NULL), ramp(NULL, SPEED)
+ {
+ }
+ void
+ init(ProductManager& pm);
+
+ ~WamNode()
+ {
+ }
+
+ bool
+ gravity(wam_srvs::GravityComp::Request &req, wam_srvs::GravityComp::Response &res);
+ bool
+ goHome(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res);
+ bool
+ holdJPos(wam_srvs::Hold::Request &req, wam_srvs::Hold::Response &res);
+ bool
+ holdCPos(wam_srvs::Hold::Request &req, wam_srvs::Hold::Response &res);
+ bool
+ holdOrtn(wam_srvs::Hold::Request &req, wam_srvs::Hold::Response &res);
+ bool
+ jointMove(wam_srvs::JointMove::Request &req, wam_srvs::JointMove::Response &res);
+ bool
+ poseMove(wam_srvs::PoseMove::Request &req, wam_srvs::PoseMove::Response &res);
+ bool
+ cartMove(wam_srvs::CartPosMove::Request &req, wam_srvs::CartPosMove::Response &res);
+ bool
+ ortnMove(wam_srvs::OrtnMove::Request &req, wam_srvs::OrtnMove::Response &res);
+ bool
+ handOpenGrasp(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res);
+ bool
+ handCloseGrasp(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res);
+ bool
+ handOpenSpread(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res);
+ bool
+ handCloseSpread(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res);
+ bool
+ handFingerPos(wam_srvs::BHandFingerPos::Request &req, wam_srvs::BHandFingerPos::Response &res);
+ bool
+ handGraspPos(wam_srvs::BHandGraspPos::Request &req, wam_srvs::BHandGraspPos::Response &res);
+ bool
+ handSpreadPos(wam_srvs::BHandSpreadPos::Request &req, wam_srvs::BHandSpreadPos::Response &res);
+ bool
+ handFingerVel(wam_srvs::BHandFingerVel::Request &req, wam_srvs::BHandFingerVel::Response &res);
+ bool
+ handGraspVel(wam_srvs::BHandGraspVel::Request &req, wam_srvs::BHandGraspVel::Response &res);
+ bool
+ handSpreadVel(wam_srvs::BHandSpreadVel::Request &req, wam_srvs::BHandSpreadVel::Response &res);
+ void
+ cartVelCB(const wam_msgs::RTCartVel::ConstPtr& msg);
+ void
+ ortnVelCB(const wam_msgs::RTOrtnVel::ConstPtr& msg);
+ void
+ jntVelCB(const wam_msgs::RTJointVel::ConstPtr& msg);
+ void
+ jntPosCB(const wam_msgs::RTJointPos::ConstPtr& msg);
+ void
+ cartPosCB(const wam_msgs::RTCartPos::ConstPtr& msg);
+ void
+ publishWam(ProductManager& pm);
+ void
+ publishHand(void);
+ void
+ updateRT(ProductManager& pm);
+ };
+
+// Templated Initialization Function
+template<size_t DOF>
+ void WamNode<DOF>::init(ProductManager& pm)
+ {
+ ros::NodeHandle n_("wam"); // WAM specific nodehandle
+ ros::NodeHandle nh_("bhand"); // BarrettHand specific nodehandle
+
+ //Setting up real-time command timeouts and initial values
+ cart_vel_status = false; //Bool for determining cartesian velocity real-time state
+ ortn_vel_status = false; //Bool for determining orientation velocity real-time state
+ new_rt_cmd = false; //Bool for determining if a new real-time message was received
+ rt_msg_timeout.fromSec(0.3); //rt_status will be determined false if rt message is not received in specified time
+ cart_vel_mag = SPEED; //Setting default cartesian velocity magnitude to SPEED
+ ortn_vel_mag = SPEED;
+ pm.getExecutionManager()->startManaging(ramp); //starting ramp manager
+
+ ROS_INFO(" \n %zu-DOF WAM", DOF);
+ jp_home = wam.getJointPositions();
+
+ if (pm.foundHand()) //Does the following only if a BarrettHand is present
+ {
+ std::cout << "Barrett Hand" << std::endl;
+ hand = pm.getHand();
+
+ // Adjust the torque limits to allow for BarrettHand movements at extents
+ pm.getSafetyModule()->setTorqueLimit(3.0);
+
+ // Move j3 in order to give room for hand initialization
+ jp_type jp_init = wam.getJointPositions();
+ jp_init[3] -= 0.35;
+ usleep(500000);
+ wam.moveTo(jp_init);
+
+ usleep(500000);
+ hand->initialize();
+ hand->update();
+
+ //Publishing the following topics only if there is a BarrettHand present
+ bhand_joint_state_pub = nh_.advertise < sensor_msgs::JointState > ("joint_states", 1); // bhand/joint_states
+
+ //Advertise the following services only if there is a BarrettHand present
+ hand_open_grsp_srv = nh_.advertiseService("open_grasp", &WamNode<DOF>::handOpenGrasp, this); // bhand/open_grasp
+ hand_close_grsp_srv = nh_.advertiseService("close_grasp", &WamNode::handCloseGrasp, this); // bhand/close_grasp
+ hand_open_sprd_srv = nh_.advertiseService("open_spread", &WamNode::handOpenSpread, this); // bhand/open_spread
+ hand_close_sprd_srv = nh_.advertiseService("close_spread", &WamNode::handCloseSpread, this); // bhand/close_spread
+ hand_fngr_pos_srv = nh_.advertiseService("finger_pos", &WamNode::handFingerPos, this); // bhand/finger_pos
+ hand_grsp_pos_srv = nh_.advertiseService("grasp_pos", &WamNode::handGraspPos, this); // bhand/grasp_pos
+ hand_sprd_pos_srv = nh_.advertiseService("spread_pos", &WamNode::handSpreadPos, this); // bhand/spread_pos
+ hand_fngr_vel_srv = nh_.advertiseService("finger_vel", &WamNode::handFingerVel, this); // bhand/finger_vel
+ hand_grsp_vel_srv = nh_.advertiseService("grasp_vel", &WamNode::handGraspVel, this); // bhand/grasp_vel
+ hand_sprd_vel_srv = nh_.advertiseService("spread_vel", &WamNode::handSpreadVel, this); // bhand/spread_vel
+
+ //Set up the BarrettHand joint state publisher
+ const char* bhand_jnts[] = {"inner_f1", "inner_f2", "inner_f3", "spread", "outer_f1", "outer_f2", "outer_f3"};
+ std::vector < std::string > bhand_joints(bhand_jnts, bhand_jnts + 7);
+ bhand_joint_state.name.resize(7);
+ bhand_joint_state.name = bhand_joints;
+ bhand_joint_state.position.resize(7);
+ }
+
+ wam.gravityCompensate(true); // Turning on Gravity Compenstation by Default when starting the WAM Node
+
+ //Setting up WAM joint state publisher
+ const char* wam_jnts[] = {"wam_j1", "wam_j2", "wam_j3", "wam_j4", "wam_j5", "wam_j6", "wam_j7"};
+ std::vector < std::string > wam_joints(wam_jnts, wam_jnts + 7);
+ wam_joint_state.name = wam_joints;
+ wam_joint_state.name.resize(DOF);
+ wam_joint_state.position.resize(DOF);
+ wam_joint_state.velocity.resize(DOF);
+ wam_joint_state.effort.resize(DOF);
+
+ //Publishing the following rostopics
+ wam_joint_state_pub = n_.advertise < sensor_msgs::JointState > ("joint_states", 1); // wam/joint_states
+ wam_pose_pub = n_.advertise < geometry_msgs::PoseStamped > ("pose", 1); // wam/pose
+
+ //Subscribing to the following rostopics
+ cart_vel_sub = n_.subscribe("cart_vel_cmd", 1, &WamNode::cartVelCB, this); // wam/cart_vel_cmd
+ ortn_vel_sub = n_.subscribe("ortn_vel_cmd", 1, &WamNode::ortnVelCB, this); // wam/ortn_vel_cmd
+ jnt_vel_sub = n_.subscribe("jnt_vel_cmd", 1, &WamNode::jntVelCB, this); // wam/jnt_vel_cmd
+ jnt_pos_sub = n_.subscribe("jnt_pos_cmd", 1, &WamNode::jntPosCB, this); // wam/jnt_pos_cmd
+ cart_pos_sub = n_.subscribe("cart_pos_cmd", 1, &WamNode::cartPosCB, this); // wam/cart_pos_cmd
+
+ //Advertising the following rosservices
+ gravity_srv = n_.advertiseService("gravity_comp", &WamNode::gravity, this); // wam/gravity_comp
+ go_home_srv = n_.advertiseService("go_home", &WamNode::goHome, this); // wam/go_home
+ hold_jpos_srv = n_.advertiseService("hold_joint_pos", &WamNode::holdJPos, this); // wam/hold_joint_pos
+ hold_cpos_srv = n_.advertiseService("hold_cart_pos", &WamNode::holdCPos, this); // wam/hold_cart_pos
+ hold_ortn_srv = n_.advertiseService("hold_ortn", &WamNode::holdOrtn, this); // wam/hold_ortn
+ joint_move_srv = n_.advertiseService("joint_move", &WamNode::jointMove, this); // wam/joint_move
+ pose_move_srv = n_.advertiseService("pose_move", &WamNode::poseMove, this); // wam/pose_move
+ cart_move_srv = n_.advertiseService("cart_move", &WamNode::cartMove, this); // wam/cart_pos_move
+ ortn_move_srv = n_.advertiseService("ortn_move", &WamNode::ortnMove, this); // wam/ortn_move
+
+ }
+
+// gravity_comp service callback
+template<size_t DOF>
+ bool WamNode<DOF>::gravity(wam_srvs::GravityComp::Request &req, wam_srvs::GravityComp::Response &res)
+ {
+ wam.gravityCompensate(req.gravity);
+ ROS_INFO("Gravity Compensation Request: %s", (req.gravity) ? "true" : "false");
+ return true;
+ }
+
+// goHome Function for sending the WAM safely back to its home starting position.
+template<size_t DOF>
+ bool WamNode<DOF>::goHome(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res)
+ {
+ ROS_INFO("Returning to Home Position");
+
+ if (hand != NULL)
+ {
+ hand->open(Hand::GRASP, true);
+ hand->close(Hand::SPREAD, true);
+ }
+ for (size_t i = 0; i < DOF; i++)
+ jp_cmd[i] = 0.0;
+ wam.moveTo(jp_cmd, true);
+ jp_home[3] -= 0.3;
+ wam.moveTo(jp_home, true);
+ jp_home[3] += 0.3;
+ wam.moveTo(jp_home, true);
+ return true;
+ }
+
+//Function to hold WAM Joint Positions
+template<size_t DOF>
+ bool WamNode<DOF>::holdJPos(wam_srvs::Hold::Request &req, wam_srvs::Hold::Response &res)
+ {
+ ROS_INFO("Joint Position Hold request: %s", (req.hold) ? "true" : "false");
+
+ if (req.hold)
+ wam.moveTo(wam.getJointPositions());
+ else
+ wam.idle();
+ return true;
+ }
+
+//Function to hold WAM end effector Cartesian Position
+template<size_t DOF>
+ bool WamNode<DOF>::holdCPos(wam_srvs::Hold::Request &req, wam_srvs::Hold::Response &res)
+ {
+ ROS_INFO("Cartesian Position Hold request: %s", (req.hold) ? "true" : "false");
+
+ if (req.hold)
+ wam.moveTo(wam.getToolPosition());
+ else
+ wam.idle();
+ return true;
+ }
+
+//Function to hold WAM end effector Orientation
+template<size_t DOF>
+ bool WamNode<DOF>::holdOrtn(wam_srvs::Hold::Request &req, wam_srvs::Hold::Response &res)
+ {
+ ROS_INFO("Orientation Hold request: %s", (req.hold) ? "true" : "false");
+
+ if (req.hold)
+ {
+ orientationSetPoint.setValue(wam.getToolOrientation());
+ wam.trackReferenceSignal(orientationSetPoint.output);
+ }
+ else
+ wam.idle();
+ return true;
+ }
+
+//Function to command a joint space move to the WAM
+template<size_t DOF>
+ bool WamNode<DOF>::jointMove(wam_srvs::JointMove::Request &req, wam_srvs::JointMove::Response &res)
+ {
+ if (req.joints.size() != DOF)
+ {
+ ROS_INFO("Request Failed: %zu-DOF request received, must be %zu-DOF", req.joints.size(), DOF);
+ return false;
+ }
+ ROS_INFO("Moving Robot to Commanded Joint Pose");
+ for (size_t i = 0; i < DOF; i++)
+ jp_cmd[i] = req.joints[i];
+ wam.moveTo(jp_cmd, false);
+ return true;
+ }
+
+//Function to command a pose move to the WAM
+template<size_t DOF>
+ bool WamNode<DOF>::poseMove(wam_srvs::PoseMove::Request &req, wam_srvs::PoseMove::Response &res)
+ {
+ ROS_INFO("Moving Robot to Commanded Pose");
+
+ cp_cmd[0] = req.pose.position.x;
+ cp_cmd[1] = req.pose.position.y;
+ cp_cmd[2] = req.pose.position.z;
+ ortn_cmd.x() = req.pose.orientation.x;
+ ortn_cmd.y() = req.pose.orientation.y;
+ ortn_cmd.z() = req.pose.orientation.z;
+ ortn_cmd.w() = req.pose.orientation.w;
+
+ pose_cmd = boost::make_tuple(cp_cmd, ortn_cmd);
+
+ //wam.moveTo(pose_cmd, false); //(TODO:KM Update Libbarrett API for Pose Moves)
+ ROS_INFO("Pose Commands for WAM not yet supported by API");
+ return false;
+ }
+
+//Function to command a cartesian move to the WAM
+template<size_t DOF>
+ bool WamNode<DOF>::cartMove(wam_srvs::CartPosMove::Request &req, wam_srvs::CartPosMove::Response &res)
+ {
+ ROS_INFO("Moving Robot to Commanded Cartesian Position");
+
+ for (int i = 0; i < 3; i++)
+ cp_cmd[i] = req.position[i];
+ wam.moveTo(cp_cmd, false);
+ return true;
+ }
+
+//Function to command an orientation move to the WAM
+template<size_t DOF>
+ bool WamNode<DOF>::ortnMove(wam_srvs::OrtnMove::Request &req, wam_srvs::OrtnMove::Response &res)
+ {
+ ROS_INFO("Moving Robot to Commanded End Effector Orientation");
+
+ ortn_cmd.x() = req.orientation[0];
+ ortn_cmd.y() = req.orientation[1];
+ ortn_cmd.z() = req.orientation[2];
+ ortn_cmd.w() = req.orientation[3];
+
+ wam.moveTo(ortn_cmd, false);
+ return true;
+ }
+
+//Function to open the BarrettHand Grasp
+template<size_t DOF>
+ bool WamNode<DOF>::handOpenGrasp(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res)
+ {
+ ROS_INFO("Opening the BarrettHand Grasp");
+ hand->open(Hand::GRASP, false);
+ return true;
+ }
+
+//Function to close the BarrettHand Grasp
+template<size_t DOF>
+ bool WamNode<DOF>::handCloseGrasp(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res)
+ {
+ ROS_INFO("Closing the BarrettHand Grasp");
+ hand->close(Hand::GRASP, false);
+ return true;
+ }
+
+//Function to open the BarrettHand Spread
+template<size_t DOF>
+ bool WamNode<DOF>::handOpenSpread(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res)
+ {
+ ROS_INFO("Opening the BarrettHand Spread");
+ hand->open(Hand::SPREAD, false);
+ return true;
+ }
+
+//Function to close the BarrettHand Spread
+template<size_t DOF>
+ bool WamNode<DOF>::handCloseSpread(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res)
+ {
+ ROS_INFO("Closing the BarrettHand Spread");
+ hand->close(Hand::SPREAD, false);
+ return true;
+ }
+
+//Function to control a BarrettHand Finger Position
+template<size_t DOF>
+ bool WamNode<DOF>::handFingerPos(wam_srvs::BHandFingerPos::Request &req, wam_srvs::BHandFingerPos::Response &res)
+ {
+ ROS_INFO("Moving BarrettHand to Finger Positions: %.3f, %.3f, %.3f radians", req.radians[0], req.radians[1],
+ req.radians[2]);
+ hand->trapezoidalMove(Hand::jp_type(req.radians[0], req.radians[1], req.radians[2], 0.0), Hand::GRASP, false);
+ return true;
+ }
+
+//Function to control the BarrettHand Grasp Position
+template<size_t DOF>
+ bool WamNode<DOF>::handGraspPos(wam_srvs::BHandGraspPos::Request &req, wam_srvs::BHandGraspPos::Response &res)
+ {
+ ROS_INFO("Moving BarrettHand Grasp: %.3f radians", req.radians);
+ hand->trapezoidalMove(Hand::jp_type(req.radians), Hand::GRASP, false);
+ return true;
+ }
+
+//Function to control the BarrettHand Spread Position
+template<size_t DOF>
+ bool WamNode<DOF>::handSpreadPos(wam_srvs::BHandSpreadPos::Request &req, wam_srvs::BHandSpreadPos::Response &res)
+ {
+ ROS_INFO("Moving BarrettHand Spread: %.3f radians", req.radians);
+ hand->trapezoidalMove(Hand::jp_type(req.radians), Hand::SPREAD, false);
+ return true;
+ }
+
+//Function to control a BarrettHand Finger Velocity
+template<size_t DOF>
+ bool WamNode<DOF>::handFingerVel(wam_srvs::BHandFingerVel::Request &req, wam_srvs::BHandFingerVel::Response &res)
+ {
+ ROS_INFO("Moving BarrettHand Finger Velocities: %.3f, %.3f, %.3f m/s", req.velocity[0], req.velocity[1],
+ req.velocity[2]);
+ hand->velocityMove(Hand::jv_type(req.velocity[0], req.velocity[1], req.velocity[2], 0.0), Hand::GRASP);
+ return true;
+ }
+
+//Function to control a BarrettHand Grasp Velocity
+template<size_t DOF>
+ bool WamNode<DOF>::handGraspVel(wam_srvs::BHandGraspVel::Request &req, wam_srvs::BHandGraspVel::Response &res)
+ {
+ ROS_INFO("Moving BarrettHand Grasp: %.3f m/s", req.velocity);
+ hand->velocityMove(Hand::jv_type(req.velocity), Hand::GRASP);
+ return true;
+ }
+
+//Function to control a BarrettHand Spread Velocity
+template<size_t DOF>
+ bool WamNode<DOF>::handSpreadVel(wam_srvs::BHandSpreadVel::Request &req, wam_srvs::BHandSpreadVel::Response &res)
+ {
+ ROS_INFO("Moving BarrettHand Spread: %.3f m/s", req.velocity);
+ usleep(5000);
+ hand->velocityMove(Hand::jv_type(req.velocity), Hand::SPREAD);
+ return true;
+ }
+
+//Callback function for RT Cartesian Velocity messages
+template<size_t DOF>
+ void WamNode<DOF>::cartVelCB(const wam_msgs::RTCartVel::ConstPtr& msg)
+ {
+ if (cart_vel_status)
+ {
+ for (size_t i = 0; i < 3; i++)
+ rt_cv_cmd[i] = msg->direction[i];
+ new_rt_cmd = true;
+ if (msg->magnitude != 0)
+ cart_vel_mag = msg->magnitude;
+ }
+ last_cart_vel_msg_time = ros::Time::now();
+
+ }
+
+//Callback function for RT Orientation Velocity Messages
+template<size_t DOF>
+ void WamNode<DOF>::ortnVelCB(const wam_msgs::RTOrtnVel::ConstPtr& msg)
+ {
+ if (ortn_vel_status)
+ {
+ for (size_t i = 0; i < 3; i++)
+ rt_ortn_cmd[i] = msg->angular[i];
+ new_rt_cmd = true;
+ if (msg->magnitude != 0)
+ ortn_vel_mag = msg->magnitude;
+ }
+ last_ortn_vel_msg_time = ros::Time::now();
+ }
+
+//Callback function for RT Joint Velocity Messages
+template<size_t DOF>
+ void WamNode<DOF>::jntVelCB(const wam_msgs::RTJointVel::ConstPtr& msg)
+ {
+ if (msg->velocities.size() != DOF)
+ {
+ ROS_INFO("Commanded Joint Velocities != DOF of WAM");
+ return;
+ }
+ if (jnt_vel_status)
+ {
+ for (size_t i = 0; i < DOF; i++)
+ rt_jv_cmd[i] = msg->velocities[i];
+ new_rt_cmd = true;
+ }
+ last_jnt_vel_msg_time = ros::Time::now();
+ }
+
+//Callback function for RT Joint Position Messages
+template<size_t DOF>
+ void WamNode<DOF>::jntPosCB(const wam_msgs::RTJointPos::ConstPtr& msg)
+ {
+ if (msg->joints.size() != DOF)
+ {
+ ROS_INFO("Commanded Joint Positions != DOF of WAM");
+ return;
+ }
+ if (jnt_pos_status)
+ {
+ for (size_t i = 0; i < DOF; i++)
+ {
+ rt_jp_cmd[i] = msg->joints[i];
+ rt_jp_rl[i] = msg->rate_limits[i];
+ }
+ new_rt_cmd = true;
+ }
+ last_jnt_pos_msg_time = ros::Time::now();
+ }
+
+//Callback function for RT Cartesian Position Messages
+template<size_t DOF>
+ void WamNode<DOF>::cartPosCB(const wam_msgs::RTCartPos::ConstPtr& msg)
+ {
+ if (cart_pos_status)
+ {
+ for (size_t i = 0; i < 3; i++)
+ {
+ rt_cp_cmd[i] = msg->position[i];
+ rt_cp_rl[i] = msg->rate_limits[i];
+ }
+ new_rt_cmd = true;
+ }
+ last_cart_pos_msg_time = ros::Time::now();
+ }
+
+//Function to update the WAM publisher
+template<size_t DOF>
+ void WamNode<DOF>::publishWam(ProductManager& pm)
+ {
+ //Current values to be published
+ jp_type jp = wam.getJointPositions();
+ jt_type jt = wam.getJointTorques();
+ jv_type jv = wam.getJointVelocities();
+ cp_type cp_pub = wam.getToolPosition();
+ Eigen::Quaterniond to_pub = wam.getToolOrientation();
+
+ //publishing sensor_msgs/JointState to wam/joint_states
+ for (size_t i = 0; i < DOF; i++)
+ {
+ wam_joint_state.position[i] = jp[i];
+ wam_joint_state.velocity[i] = jv[i];
+ wam_joint_state.effort[i] = jt[i];
+ }
+ wam_joint_state.header.stamp = ros::Time::now();
+ wam_joint_state_pub.publish(wam_joint_state);
+
+ //publishing geometry_msgs/PoseStamed to wam/pose
+ wam_pose.header.stamp = ros::Time::now();
+ wam_pose.pose.position.x = cp_pub[0];
+ wam_pose.pose.position.y = cp_pub[1];
+ wam_pose.pose.position.z = cp_pub[2];
+ wam_pose.pose.orientation.w = to_pub.w();
+ wam_pose.pose.orientation.x = to_pub.x();
+ wam_pose.pose.orientation.y = to_pub.y();
+ wam_pose.pose.orientation.z = to_pub.z();
+ wam_pose_pub.publish(wam_pose);
+ }
+
+//Function to update the real-time control loops
+template<size_t DOF>
+ void WamNode<DOF>::publishHand() //systems::PeriodicDataLogger<debug_tuple>& logger
+ {
+ while (ros::ok())
+ {
+ hand->update(); // Update the hand sensors
+ Hand::jp_type hi = hand->getInnerLinkPosition(); // get finger positions information
+ Hand::jp_type ho = hand->getOuterLinkPosition();
+ for (size_t i = 0; i < 4; i++) // Save finger positions
+ bhand_joint_state.position[i] = hi[i];
+ for (size_t j = 0; j < 3; j++)
+ bhand_joint_state.position[j + 4] = ho[j];
+ bhand_joint_state.header.stamp = ros::Time::now(); // Set the timestamp
+ bhand_joint_state_pub.publish(bhand_joint_state); // Publish the BarrettHand joint states
+ btsleep(1.0 / BHAND_PUBLISH_FREQ); // Sleep according to the specified publishing frequency
+ }
+ }
+
+//Function to update the real-time control loops
+template<size_t DOF>
+ void WamNode<DOF>::updateRT(ProductManager& pm) //systems::PeriodicDataLogger<debug_tuple>& logger
+ {
+ //Real-Time Cartesian Velocity Control Portion
+ if (last_cart_vel_msg_time + rt_msg_timeout > ros::Time::now()) // checking if a cartesian velocity message has been published and if it is within timeout
+ {
+ if (!cart_vel_status)
+ {
+ cart_dir.setValue(cp_type(0.0, 0.0, 0.0)); // zeroing the cartesian direction
+ current_cart_pos.setValue(wam.getToolPosition()); // Initializing the cartesian position
+ current_ortn.setValue(wam.getToolOrientation()); // Initializing the orientation
+ systems::forceConnect(ramp.output, mult_linear.input1); // connecting the ramp to multiplier
+ systems::forceConnect(cart_dir.output, mult_linear.input2); // connecting the direction to the multiplier
+ systems::forceConnect(mult_linear.output, cart_pos_sum.getInput(0)); // adding the output of the multiplier
+ systems::forceConnect(current_cart_pos.output, cart_pos_sum.getInput(1)); // with the starting cartesian position offset
+ systems::forceConnect(cart_pos_sum.output, rt_pose_cmd.getInput<0>()); // saving summed position as new commanded pose.position
+ systems::forceConnect(current_ortn.output, rt_pose_cmd.getInput<1>()); // saving the original orientation to the pose.orientation
+ ramp.setSlope(cart_vel_mag); // setting the slope to the commanded magnitude
+ ramp.stop(); // ramp is stopped on startup
+ ramp.setOutput(0.0); // ramp is re-zeroed on startup
+ ramp.start(); // start the ramp
+ wam.trackReferenceSignal(rt_pose_cmd.output); // command WAM to track the RT commanded (500 Hz) updated pose
+ }
+ else if (new_rt_cmd)
+ {
+ ramp.reset(); // reset the ramp to 0
+ ramp.setSlope(cart_vel_mag);
+ cart_dir.setValue(rt_cv_cmd); // set our cartesian direction to subscribed command
+ current_cart_pos.setValue(wam.tpoTpController.referenceInput.getValue()); // updating the current position to the actual low level commanded value
+ }
+ cart_vel_status = true;
+ new_rt_cmd = false;
+ }
+
+ //Real-Time Angular Velocity Control Portion
+ else if (last_ortn_vel_msg_time + rt_msg_timeout > ros::Time::now()) // checking if a orientation velocity message has been published and if it is within timeout
+ {
+ if (!ortn_vel_status)
+ {
+ rpy_cmd.setValue(math::Vector<3>::type(0.0, 0.0, 0.0)); // zeroing the rpy command
+ current_cart_pos.setValue(wam.getToolPosition()); // Initializing the cartesian position
+ current_rpy_ortn.setValue(toRPY(wam.getToolOrientation())); // Initializing the orientation
+
+ systems::forceConnect(ramp.output, mult_angular.input1); // connecting the ramp to multiplier
+ systems::forceConnect(rpy_cmd.output, mult_angular.input2); // connecting the rpy command to the multiplier
+ systems::forceConnect(mult_angular.output, ortn_cmd_sum.getInput(0)); // adding the output of the multiplier
+ systems::forceConnect(current_rpy_ortn.output, ortn_cmd_sum.getInput(1)); // with the starting rpy orientation offset
+ systems::forceConnect(ortn_cmd_sum.output, to_quat.input);
+ systems::forceConnect(current_cart_pos.output, rt_pose_cmd.getInput<0>()); // saving the original position to the pose.position
+ systems::forceConnect(to_quat.output, rt_pose_cmd.getInput<1>()); // saving the summed and converted new quaternion commmand as the pose.orientation
+ ramp.setSlope(ortn_vel_mag); // setting the slope to the commanded magnitude
+ ramp.stop(); // ramp is stopped on startup
+ ramp.setOutput(0.0); // ramp is re-zeroed on startup
+ ramp.start(); // start the ramp
+ wam.trackReferenceSignal(rt_pose_cmd.output); // command the WAM to track the RT commanded up to (500 Hz) cartesian velocity
+ }
+ else if (new_rt_cmd)
+ {
+ ramp.reset(); // reset the ramp to 0
+ ramp.setSlope(ortn_vel_mag); // updating the commanded angular velocity magnitude
+ rpy_cmd.setValue(rt_ortn_cmd); // set our angular rpy command to subscribed command
+ current_rpy_ortn.setValue(toRPY(wam.tpoToController.referenceInput.getValue())); // updating the current orientation to the actual low level commanded value
+ }
+ ortn_vel_status = true;
+ new_rt_cmd = false;
+ }
+
+ //Real-Time Joint Velocity Control Portion
+ else if (last_jnt_vel_msg_time + rt_msg_timeout > ros::Time::now()) // checking if a joint velocity message has been published and if it is within timeout
+ {
+ if (!jnt_vel_status)
+ {
+ jv_type jv_start;
+ for (size_t i = 0; i < DOF; i++)
+ jv_start[i] = 0.0;
+ jv_track.setValue(jv_start); // zeroing the joint velocity command
+ wam.trackReferenceSignal(jv_track.output); // command the WAM to track the RT commanded up to (500 Hz) joint velocities
+ }
+ else if (new_rt_cmd)
+ {
+ jv_track.setValue(rt_jv_cmd); // set our joint velocity to subscribed command
+ }
+ jnt_vel_status = true;
+ new_rt_cmd = false;
+ }
+
+ //Real-Time Joint Position Control Portion
+ else if (last_jnt_pos_msg_time + rt_msg_timeout > ros::Time::now()) // checking if a joint position message has been published and if it is within timeout
+ {
+ if (!jnt_pos_status)
+ {
+ jp_type jp_start = wam.getJointPositions();
+ jp_track.setValue(jp_start); // setting initial the joint position command
+ jp_rl.setLimit(rt_jp_rl);
+ systems::forceConnect(jp_track.output, jp_rl.input);
+ wam.trackReferenceSignal(jp_rl.output); // command the WAM to track the RT commanded up to (500 Hz) joint positions
+ }
+ else if (new_rt_cmd)
+ {
+ jp_track.setValue(rt_jp_cmd); // set our joint position to subscribed command
+ jp_rl.setLimit(rt_jp_rl); // set our rate limit to subscribed rate to control the rate of the moves
+ }
+ jnt_pos_status = true;
+ new_rt_cmd = false;
+ }
+
+ //Real-Time Cartesian Position Control Portion
+ else if (last_cart_pos_msg_time + rt_msg_timeout > ros::Time::now()) // checking if a cartesian position message has been published and if it is within timeout
+ {
+ if (!cart_pos_status)
+ {
+ cp_track.setValue(wam.getToolPosition());
+ current_ortn.setValue(wam.getToolOrientation()); // Initializing the orientation
+ cp_rl.setLimit(rt_cp_rl);
+ systems::forceConnect(cp_track.output, cp_rl.input);
+ systems::forceConnect(cp_rl.output, rt_pose_cmd.getInput<0>()); // saving the rate limited cartesian position command to the pose.position
+ systems::forceConnect(current_ortn.output, rt_pose_cmd.getInput<1>()); // saving the original orientation to the pose.orientation
+ wam.trackReferenceSignal(rt_pose_cmd.output); //Commanding the WAM to track the real-time pose command.
+ }
+ else if (new_rt_cmd)
+ {
+ cp_track.setValue(rt_cp_cmd); // Set our cartesian positions to subscribed command
+ cp_rl.setLimit(rt_cp_rl); // Updating the rate limit to subscribed rate to control the rate of the moves
+ }
+ cart_pos_status = true;
+ new_rt_cmd = false;
+ }
+
+ //If we fall out of 'Real-Time', hold joint positions
+ else if (cart_vel_status | ortn_vel_status | jnt_vel_status | jnt_pos_status | cart_pos_status)
+ {
+ wam.moveTo(wam.getJointPositions()); // Holds current joint positions upon a RT message timeout
+ cart_vel_status = ortn_vel_status = jnt_vel_status = jnt_pos_status = cart_pos_status = ortn_pos_status = false;
+ }
+ }
+
+//wam_main Function
+template<size_t DOF>
+ int wam_main(int argc, char** argv, ProductManager& pm, systems::Wam<DOF>& wam)
+ {
+ BARRETT_UNITS_TEMPLATE_TYPEDEFS(DOF);
+ ros::init(argc, argv, "wam_node");
+ WamNode<DOF> wam_node(wam);
+ wam_node.init(pm);
+ ros::Rate pub_rate(PUBLISH_FREQ);
+
+ if (pm.getHand())
+ boost::thread handPubThread(&WamNode<DOF>::publishHand, &wam_node);
+
+ while (ros::ok() && pm.getSafetyModule()->getMode() == SafetyModule::ACTIVE)
+ {
+ ros::spinOnce();
+ wam_node.publishWam(pm);
+ wam_node.updateRT(pm);
+ pub_rate.sleep();
+ }
+
+ return 0;
+ }
--- /dev/null
+cmake_minimum_required(VERSION 2.8.3)
+project(wam_robot)
+find_package(catkin REQUIRED)
+catkin_metapackage()
--- /dev/null
+<?xml version="1.0"?>
+<package>
+ <name>wam_robot</name>
+ <version>2.0.0</version>
+ <description>The wam_robot package
+ The following stack describing packages designed to run on a
+ 4-DOF or 7-DOF WAM with or without a BarrettHand via the onboard WAM PC or
+ external control PC.
+ Ported from the ROS Fuerte version to Indigo by Walter Fetter Lages.
+ </description>
+
+ <!-- One maintainer tag required, multiple allowed, one person per tag -->
+ <!-- Example: -->
+ <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
+ <maintainer email="fetter@ece.ufrgs.br">Walter Fetter Lages</maintainer>
+
+
+ <!-- One license tag required, multiple allowed, one license per tag -->
+ <!-- Commonly used license strings: -->
+ <!-- BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
+ <license>GPL</license>
+
+
+ <!-- Url tags are optional, but multiple are allowed, one per tag -->
+ <!-- Optional attribute type can be: website, bugtracker, or repository -->
+ <!-- Example: -->
+ <!-- <url type="website">http://wiki.ros.org/wam_common</url> -->
+ <url type="website">http://ros.org/wiki/wam_robot</url>
+
+ <!-- Author tags are optional, multiple are allowed, one per tag -->
+ <!-- Authors do not have to be maintainers, but could be -->
+ <!-- Example: -->
+ <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
+ <author>Barrett Technology Inc., Kyle Maroney</author>
+
+ <!-- The *_depend tags are used to specify dependencies -->
+ <!-- Dependencies can be catkin packages or system dependencies -->
+ <!-- Examples: -->
+ <!-- Use build_depend for packages you need at compile time: -->
+ <!-- <build_depend>message_generation</build_depend> -->
+ <!-- Use buildtool_depend for build tool packages: -->
+ <!-- <buildtool_depend>catkin</buildtool_depend> -->
+ <!-- Use run_depend for packages you need at runtime: -->
+ <!-- <run_depend>message_runtime</run_depend> -->
+ <!-- Use test_depend for packages you need only for testing: -->
+ <!-- <test_depend>gtest</test_depend> -->
+ <buildtool_depend>catkin</buildtool_depend>
+ <run_depend>wam_node</run_depend>
+ <run_depend>wam_srvs</run_depend>
+ <run_depend>wam_teleop</run_depend>
+
+
+ <!-- The export tag contains other, unspecified, tags -->
+ <export>
+ <!-- Other tools can request additional information be placed here -->
+ <metapackage/>
+ </export>
+</package>
--- /dev/null
+cmake_minimum_required(VERSION 2.8.3)
+project(wam_srvs)
+
+## Compile as C++11, supported in ROS Kinetic and newer
+# add_compile_options(-std=c++11)
+
+## Find catkin macros and libraries
+## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
+## is used, also find other catkin packages
+find_package(catkin REQUIRED COMPONENTS
+ geometry_msgs
+ message_generation
+)
+
+## System dependencies are found with CMake's conventions
+# find_package(Boost REQUIRED COMPONENTS system)
+
+
+## Uncomment this if the package has a setup.py. This macro ensures
+## modules and global scripts declared therein get installed
+## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
+# catkin_python_setup()
+
+################################################
+## Declare ROS messages, services and actions ##
+################################################
+
+## To declare and build messages, services or actions from within this
+## package, follow these steps:
+## * Let MSG_DEP_SET be the set of packages whose message types you use in
+## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
+## * In the file package.xml:
+## * add a build_depend tag for "message_generation"
+## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET
+## * If MSG_DEP_SET isn't empty the following dependency has been pulled in
+## but can be declared for certainty nonetheless:
+## * add a run_depend tag for "message_runtime"
+## * In this file (CMakeLists.txt):
+## * add "message_generation" and every package in MSG_DEP_SET to
+## find_package(catkin REQUIRED COMPONENTS ...)
+## * add "message_runtime" and every package in MSG_DEP_SET to
+## catkin_package(CATKIN_DEPENDS ...)
+## * uncomment the add_*_files sections below as needed
+## and list every .msg/.srv/.action file to be processed
+## * uncomment the generate_messages entry below
+## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
+
+## Generate messages in the 'msg' folder
+# add_message_files(
+# FILES
+# Message1.msg
+# Message2.msg
+# )
+
+## Generate services in the 'srv' folder
+add_service_files(
+ FILES
+# Service1.srv
+# Service2.srv
+ BHandFingerPos.srv
+ BHandFingerVel.srv
+ BHandGraspPos.srv
+ BHandGraspVel.srv
+ BHandSpreadPos.srv
+ BHandSpreadVel.srv
+ CartPosMove.srv
+ GravityComp.srv
+ Hold.srv
+ JointMove.srv
+ OrtnMove.srv
+ PoseMove.srv
+)
+
+## Generate actions in the 'action' folder
+# add_action_files(
+# FILES
+# Action1.action
+# Action2.action
+# )
+
+## Generate added messages and services with any dependencies listed here
+generate_messages(
+ DEPENDENCIES
+ geometry_msgs
+)
+
+################################################
+## Declare ROS dynamic reconfigure parameters ##
+################################################
+
+## To declare and build dynamic reconfigure parameters within this
+## package, follow these steps:
+## * In the file package.xml:
+## * add a build_depend and a run_depend tag for "dynamic_reconfigure"
+## * In this file (CMakeLists.txt):
+## * add "dynamic_reconfigure" to
+## find_package(catkin REQUIRED COMPONENTS ...)
+## * uncomment the "generate_dynamic_reconfigure_options" section below
+## and list every .cfg file to be processed
+
+## Generate dynamic reconfigure parameters in the 'cfg' folder
+# generate_dynamic_reconfigure_options(
+# cfg/DynReconf1.cfg
+# cfg/DynReconf2.cfg
+# )
+
+###################################
+## catkin specific configuration ##
+###################################
+## The catkin_package macro generates cmake config files for your package
+## Declare things to be passed to dependent projects
+## INCLUDE_DIRS: uncomment this if you package contains header files
+## LIBRARIES: libraries you create in this project that dependent projects also need
+## CATKIN_DEPENDS: catkin_packages dependent projects also need
+## DEPENDS: system dependencies of this project that dependent projects also need
+catkin_package(
+# INCLUDE_DIRS include
+# LIBRARIES wam_srvs
+ CATKIN_DEPENDS geometry_msgs message_runtime
+# DEPENDS system_lib
+)
+
+###########
+## Build ##
+###########
+
+## Specify additional locations of header files
+## Your package locations should be listed before other locations
+include_directories(
+# include
+ ${catkin_INCLUDE_DIRS}
+)
+
+## Declare a C++ library
+# add_library(${PROJECT_NAME}
+# src/${PROJECT_NAME}/wam_srvs.cpp
+# )
+
+## Add cmake target dependencies of the library
+## as an example, code may need to be generated before libraries
+## either from message generation or dynamic reconfigure
+# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Declare a C++ executable
+## With catkin_make all packages are built within a single CMake context
+## The recommended prefix ensures that target names across packages don't collide
+# add_executable(${PROJECT_NAME}_node src/wam_srvs_node.cpp)
+
+## Rename C++ executable without prefix
+## The above recommended prefix causes long target names, the following renames the
+## target back to the shorter version for ease of user use
+## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
+# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "")
+
+## Add cmake target dependencies of the executable
+## same as for the library above
+# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Specify libraries to link a library or executable target against
+# target_link_libraries(${PROJECT_NAME}_node
+# ${catkin_LIBRARIES}
+# )
+
+#############
+## Install ##
+#############
+
+# all install targets should use catkin DESTINATION variables
+# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
+
+## Mark executable scripts (Python etc.) for installation
+## in contrast to setup.py, you can choose the destination
+# install(PROGRAMS
+# scripts/my_python_script
+# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark executables and/or libraries for installation
+# install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node
+# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark cpp header files for installation
+# install(DIRECTORY include/${PROJECT_NAME}/
+# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+# FILES_MATCHING PATTERN "*.h"
+# PATTERN ".svn" EXCLUDE
+# )
+
+## Mark other files for installation (e.g. launch and bag files, etc.)
+# install(FILES
+# # myfile1
+# # myfile2
+# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+# )
+
+#############
+## Testing ##
+#############
+
+## Add gtest based cpp test target and link libraries
+# catkin_add_gtest(${PROJECT_NAME}-test test/test_wam_srvs.cpp)
+# if(TARGET ${PROJECT_NAME}-test)
+# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
+# endif()
+
+## Add folders to be run by python nosetests
+# catkin_add_nosetests(test)
--- /dev/null
+<?xml version="1.0"?>
+<package>
+ <name>wam_srvs</name>
+ <version>0.0.0</version>
+ <description>The wam_srvs package
+ This Package contains WAM / BarrettHand specific service definitions.
+ Ported from the ROS Fuerte version to Indigo by Walter Fetter Lages.
+ </description>
+
+ <!-- One maintainer tag required, multiple allowed, one person per tag -->
+ <!-- Example: -->
+ <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
+ <maintainer email="fetter@ece.ufrgs.br">Walter Fetter Lages</maintainer>
+
+
+ <!-- One license tag required, multiple allowed, one license per tag -->
+ <!-- Commonly used license strings: -->
+ <!-- BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
+ <license>BSD</license>
+
+
+ <!-- Url tags are optional, but multiple are allowed, one per tag -->
+ <!-- Optional attribute type can be: website, bugtracker, or repository -->
+ <!-- Example: -->
+ <!-- <url type="website">http://wiki.ros.org/wam_srvs</url> -->
+
+
+ <!-- Author tags are optional, multiple are allowed, one per tag -->
+ <!-- Authors do not have to be maintainers, but could be -->
+ <!-- Example: -->
+ <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
+ <author>Barrett Technology Inc., Kyle Maroney</author>
+
+ <!-- The *_depend tags are used to specify dependencies -->
+ <!-- Dependencies can be catkin packages or system dependencies -->
+ <!-- Examples: -->
+ <!-- Use build_depend for packages you need at compile time: -->
+ <!-- <build_depend>message_generation</build_depend> -->
+ <!-- Use buildtool_depend for build tool packages: -->
+ <!-- <buildtool_depend>catkin</buildtool_depend> -->
+ <!-- Use run_depend for packages you need at runtime: -->
+ <!-- <run_depend>message_runtime</run_depend> -->
+ <!-- Use test_depend for packages you need only for testing: -->
+ <!-- <test_depend>gtest</test_depend> -->
+ <buildtool_depend>catkin</buildtool_depend>
+ <build_depend>geometry_msgs</build_depend>
+ <build_depend>message_generation</build_depend>
+ <run_depend>geometry_msgs</run_depend>
+ <run_depend>message_runtime</run_depend>
+
+
+ <!-- The export tag contains other, unspecified, tags -->
+ <export>
+ <!-- Other tools can request additional information be placed here -->
+
+ </export>
+</package>
--- /dev/null
+float32[3] radians
+---
--- /dev/null
+float32[3] velocity
+---
--- /dev/null
+float32 radians
+---
--- /dev/null
+float32 velocity
+---
--- /dev/null
+float32 radians
+---
--- /dev/null
+float32 velocity
+---
--- /dev/null
+float32[3] position
+---
\ No newline at end of file
--- /dev/null
+bool gravity
+---
--- /dev/null
+bool hold
+---
--- /dev/null
+float32[] joints
+---
--- /dev/null
+float32[4] orientation
+---
\ No newline at end of file
--- /dev/null
+geometry_msgs/Pose pose
+---
--- /dev/null
+cmake_minimum_required(VERSION 2.8.3)
+project(wam_teleop)
+
+## Compile as C++11, supported in ROS Kinetic and newer
+# add_compile_options(-std=c++11)
+
+## Find catkin macros and libraries
+## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
+## is used, also find other catkin packages
+find_package(catkin REQUIRED COMPONENTS
+ joy
+ roscpp
+ sensor_msgs
+ std_srvs
+ wam_msgs
+ wam_srvs
+)
+
+## System dependencies are found with CMake's conventions
+# find_package(Boost REQUIRED COMPONENTS system)
+
+
+## Uncomment this if the package has a setup.py. This macro ensures
+## modules and global scripts declared therein get installed
+## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
+# catkin_python_setup()
+
+################################################
+## Declare ROS messages, services and actions ##
+################################################
+
+## To declare and build messages, services or actions from within this
+## package, follow these steps:
+## * Let MSG_DEP_SET be the set of packages whose message types you use in
+## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
+## * In the file package.xml:
+## * add a build_depend tag for "message_generation"
+## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET
+## * If MSG_DEP_SET isn't empty the following dependency has been pulled in
+## but can be declared for certainty nonetheless:
+## * add a run_depend tag for "message_runtime"
+## * In this file (CMakeLists.txt):
+## * add "message_generation" and every package in MSG_DEP_SET to
+## find_package(catkin REQUIRED COMPONENTS ...)
+## * add "message_runtime" and every package in MSG_DEP_SET to
+## catkin_package(CATKIN_DEPENDS ...)
+## * uncomment the add_*_files sections below as needed
+## and list every .msg/.srv/.action file to be processed
+## * uncomment the generate_messages entry below
+## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
+
+## Generate messages in the 'msg' folder
+# add_message_files(
+# FILES
+# Message1.msg
+# Message2.msg
+# )
+
+## Generate services in the 'srv' folder
+# add_service_files(
+# FILES
+# Service1.srv
+# Service2.srv
+# )
+
+## Generate actions in the 'action' folder
+# add_action_files(
+# FILES
+# Action1.action
+# Action2.action
+# )
+
+## Generate added messages and services with any dependencies listed here
+# generate_messages(
+# DEPENDENCIES
+# sensor_msgs# wam_msgs
+# )
+
+################################################
+## Declare ROS dynamic reconfigure parameters ##
+################################################
+
+## To declare and build dynamic reconfigure parameters within this
+## package, follow these steps:
+## * In the file package.xml:
+## * add a build_depend and a run_depend tag for "dynamic_reconfigure"
+## * In this file (CMakeLists.txt):
+## * add "dynamic_reconfigure" to
+## find_package(catkin REQUIRED COMPONENTS ...)
+## * uncomment the "generate_dynamic_reconfigure_options" section below
+## and list every .cfg file to be processed
+
+## Generate dynamic reconfigure parameters in the 'cfg' folder
+# generate_dynamic_reconfigure_options(
+# cfg/DynReconf1.cfg
+# cfg/DynReconf2.cfg
+# )
+
+###################################
+## catkin specific configuration ##
+###################################
+## The catkin_package macro generates cmake config files for your package
+## Declare things to be passed to dependent projects
+## INCLUDE_DIRS: uncomment this if you package contains header files
+## LIBRARIES: libraries you create in this project that dependent projects also need
+## CATKIN_DEPENDS: catkin_packages dependent projects also need
+## DEPENDS: system dependencies of this project that dependent projects also need
+catkin_package(
+# INCLUDE_DIRS include
+# LIBRARIES wam_teleop
+# CATKIN_DEPENDS joy roscpp sensor_msgs std_srvs wam_msgs wam_srvs
+# DEPENDS system_lib
+)
+
+###########
+## Build ##
+###########
+
+## Specify additional locations of header files
+## Your package locations should be listed before other locations
+include_directories(
+# include
+ ${catkin_INCLUDE_DIRS}
+)
+
+## Declare a C++ library
+# add_library(${PROJECT_NAME}
+# src/${PROJECT_NAME}/wam_teleop.cpp
+# )
+
+## Add cmake target dependencies of the library
+## as an example, code may need to be generated before libraries
+## either from message generation or dynamic reconfigure
+# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Declare a C++ executable
+## With catkin_make all packages are built within a single CMake context
+## The recommended prefix ensures that target names across packages don't collide
+add_executable(wam_joystick_teleop src/wam_joystick_teleop.cpp)
+
+## Rename C++ executable without prefix
+## The above recommended prefix causes long target names, the following renames the
+## target back to the shorter version for ease of user use
+## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
+# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "")
+
+## Add cmake target dependencies of the executable
+## same as for the library above
+# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Specify libraries to link a library or executable target against
+target_link_libraries(wam_joystick_teleop
+ ${catkin_LIBRARIES}
+)
+
+#############
+## Install ##
+#############
+
+# all install targets should use catkin DESTINATION variables
+# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
+
+## Mark executable scripts (Python etc.) for installation
+## in contrast to setup.py, you can choose the destination
+# install(PROGRAMS
+# scripts/my_python_script
+# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark executables and/or libraries for installation
+# install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node
+# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark cpp header files for installation
+# install(DIRECTORY include/${PROJECT_NAME}/
+# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+# FILES_MATCHING PATTERN "*.h"
+# PATTERN ".svn" EXCLUDE
+# )
+
+## Mark other files for installation (e.g. launch and bag files, etc.)
+# install(FILES
+# # myfile1
+# # myfile2
+# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+# )
+
+#############
+## Testing ##
+#############
+
+## Add gtest based cpp test target and link libraries
+# catkin_add_gtest(${PROJECT_NAME}-test test/test_wam_teleop.cpp)
+# if(TARGET ${PROJECT_NAME}-test)
+# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
+# endif()
+
+## Add folders to be run by python nosetests
+# catkin_add_nosetests(test)
--- /dev/null
+<launch>
+
+ <!-- joy node -->
+ <node name="joy" pkg="joy" type="joy_node" respawn="true">
+ <param name="dev" type="string" value="/dev/input/js0" />
+ <param name="deadzone" value="0.12" />
+ </node>
+
+ <!-- WAM Teleop Node -->
+ <node pkg="wam_teleop" type="wam_joystick_teleop" name="wam_joystick_teleop" output="screen"/>
+
+</launch>
--- /dev/null
+<launch>
+
+ <!-- joy node -->
+ <node name="joy" pkg="joy" type="joy_node" respawn="true">
+ <param name="dev" type="string" value="/dev/input/js0" />
+ <param name="deadzone" value="0.12" />
+ </node>
+
+ <!-- WAM Teleop Node -->
+ <node pkg="wam_teleop" type="wam_joystick_teleop" name="wam_joystick_teleop" output="screen" />
+
+ <param name="deadman_button" type="int" value="0" /> <!-- fire -->
+ <param name="guardian_deadman_button" type="int" value="4" /> <!-- O5 -->
+
+ <param name="gripper_open_button" type="int" value="9" /> <!-- H2 up -->
+ <param name="gripper_close_button" type="int" value="11" /> <!-- H2 down -->
+ <param name="spread_open_button" type="int" value="10" /> <!-- H2 right -->
+ <param name="spread_close_button" type="int" value="12" /> <!-- H2 left -->
+
+ <param name="orientation_control_button" type="int" value="3" /> <!-- O4 -->
+ <param name="go_home_button" type="int" value="1" /> <!-- O2 -->
+ <param name="hold_joints_button" type="int" value="2" /> <!-- O3 -->
+
+ <param name="grasp_max_velocity" type="double" value="1.0" />
+ <param name="spread_max_velocity" type="double" value="1.0" />
+ <param name="cartesian_magnitude" type="double" value="0.05" />
+ <param name="orientation_magnitude" type="double" value="1.0" />
+
+ <param name="cartesian_x_axis" type="int" value="0" /> <!-- X axis -->
+ <param name="cartesian_y_axis" type="int" value="1" /> <!-- Y axis -->
+ <param name="cartesian_z_axis" type="int" value="2" /> <!-- Z axis -->
+
+ <param name="orientation_roll_axis" type="int" value="0" /> <!-- X axis -->
+ <param name="orientation_pitch_axis" type="int" value="1" /> <!-- Y axis -->
+ <param name="orientation_yaw_axis" type="int" value="2" /> <!-- Z axis -->
+</launch>
--- /dev/null
+<?xml version="1.0"?>
+<package>
+ <name>wam_teleop</name>
+ <version>2.0.0</version>
+ <description>The wam_teleop package
+ This package contains a joystick and keyboard teleoperation for the
+ Barrett WAM and BarrettHand.
+ Ported from the ROS Fuerte version to Indigo by Walter Fetter Lages.
+ </description>
+
+ <!-- One maintainer tag required, multiple allowed, one person per tag -->
+ <!-- Example: -->
+ <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
+ <maintainer email="fetter@ece.ufrgs.br">Walter Fetter Lages</maintainer>
+
+
+ <!-- One license tag required, multiple allowed, one license per tag -->
+ <!-- Commonly used license strings: -->
+ <!-- BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
+ <license>BSD</license>
+
+
+ <!-- Url tags are optional, but multiple are allowed, one per tag -->
+ <!-- Optional attribute type can be: website, bugtracker, or repository -->
+ <!-- Example: -->
+ <!-- <url type="website">http://wiki.ros.org/wam_teleop</url> -->
+
+
+ <!-- Author tags are optional, multiple are allowed, one per tag -->
+ <!-- Authors do not have to be maintainers, but could be -->
+ <!-- Example: -->
+ <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
+ <author>Barrett Technology Inc., Kyle Maroney</author>
+
+ <!-- The *_depend tags are used to specify dependencies -->
+ <!-- Dependencies can be catkin packages or system dependencies -->
+ <!-- Examples: -->
+ <!-- Use build_depend for packages you need at compile time: -->
+ <!-- <build_depend>message_generation</build_depend> -->
+ <!-- Use buildtool_depend for build tool packages: -->
+ <!-- <buildtool_depend>catkin</buildtool_depend> -->
+ <!-- Use run_depend for packages you need at runtime: -->
+ <!-- <run_depend>message_runtime</run_depend> -->
+ <!-- Use test_depend for packages you need only for testing: -->
+ <!-- <test_depend>gtest</test_depend> -->
+ <buildtool_depend>catkin</buildtool_depend>
+ <build_depend>joy</build_depend>
+ <build_depend>roscpp</build_depend>
+ <build_depend>sensor_msgs</build_depend>
+ <build_depend>std_srvs</build_depend>
+ <build_depend>wam_msgs</build_depend>
+ <build_depend>wam_srvs</build_depend>
+ <run_depend>joy</run_depend>
+ <run_depend>roscpp</run_depend>
+ <run_depend>sensor_msgs</run_depend>
+ <run_depend>std_srvs</run_depend>
+ <run_depend>wam_msgs</run_depend>
+ <run_depend>wam_srvs</run_depend>
+
+
+ <!-- The export tag contains other, unspecified, tags -->
+ <export>
+ <!-- Other tools can request additional information be placed here -->
+
+ </export>
+</package>
--- /dev/null
+#include <math.h>
+#include <ros/ros.h>
+
+#include "geometry_msgs/PoseStamped.h"
+#include "sensor_msgs/Joy.h"
+#include "std_srvs/Empty.h"
+
+#include "wam_msgs/RTCartVel.h"
+#include "wam_msgs/RTOrtnVel.h"
+#include "wam_srvs/GravityComp.h"
+#include "wam_srvs/Hold.h"
+#include "wam_srvs/BHandGraspVel.h"
+#include "wam_srvs/BHandSpreadVel.h"
+
+const int CNTRL_FREQ = 50; // Frequency at which we will publish our control messages.
+
+//WamTeleop Class
+class WamTeleop
+{
+public:
+ ros::NodeHandle n_, nw_, nh_; // NodeHandles for publishing / subscribing on topics "/... & /wam/..." & "/bhand/..."
+
+ // Boolean statuses for commanded states
+ bool pose_pubbed, grsp_publish, sprd_publish;
+ bool cart_publish, ortn_publish, home_publish;
+ bool hold_publish, home_st, hold_st, ortn_mode;
+
+ // Integer status for BarrettHand commanded state
+ int bh_cmd_st;
+
+ //variables to describe buttons on Joystick and their assignments
+ int deadman_btn, guardian_deadman_btn;
+ int gpr_open_btn, gpr_close_btn;
+ int sprd_open_btn, sprd_close_btn;
+ int ortn_btn, home_btn, hold_btn;
+ int axis_x, axis_y, axis_z;
+ int axis_r, axis_p, axis_yaw;
+ //variables to describe velocity commands
+ double max_grsp_vel, max_sprd_vel;
+ double cart_mag, ortn_mag;
+ double req_xdir, req_ydir, req_zdir;
+ double req_rdir, req_pdir, req_yawdir;
+ double bh_grsp_vel, bh_sprd_vel;
+
+ //Subscribers
+ ros::Subscriber joy_sub;
+
+ //Services
+ wam_srvs::BHandGraspVel grasp_vel;
+ wam_srvs::BHandSpreadVel spread_vel;
+ wam_srvs::Hold hold;
+ std_srvs::Empty go_home;
+
+ //Service Clients
+ ros::ServiceClient grasp_vel_srv, spread_vel_srv, go_home_srv;
+ ros::ServiceClient hold_srv;
+
+ //Messages
+ wam_msgs::RTCartVel cart_vel;
+ wam_msgs::RTOrtnVel ortn_vel;
+
+ //Publishers
+ ros::Publisher cart_vel_pub, ortn_vel_pub;
+
+ WamTeleop() :
+ nw_("wam"), nh_("bhand") // Name our nodehandle "wam" to preceed our messages/services
+ {
+ }
+
+ void init();
+ void joyCallback(const sensor_msgs::Joy::ConstPtr& joy_msg);
+ void update();
+
+ ~WamTeleop()
+ {
+ }
+};
+
+// WAM Teleoperation Initialization Function
+void WamTeleop::init()
+{
+ // We will check the parameter server for WAM Teleoperation configuration variables
+ n_.param("deadman_button", deadman_btn, 10);
+ n_.param("guardian_deadman_button", guardian_deadman_btn, 11);
+ n_.param("gripper_open_button", gpr_open_btn, 12);
+ n_.param("gripper_close_button", gpr_close_btn, 14);
+ n_.param("spread_open_button", sprd_open_btn, 13);
+ n_.param("spread_close_button", sprd_close_btn, 15);
+ n_.param("orientation_control_button", ortn_btn, 8);
+ n_.param("go_home_button", home_btn, 0);
+ n_.param("hold_joints_button", hold_btn, 3);
+ n_.param("grasp_max_velocity", max_grsp_vel, 1.0);
+ n_.param("spread_max_velocity", max_sprd_vel, 1.0);
+ n_.param("cartesian_magnitude", cart_mag, 0.05);
+ n_.param("orientation_magnitude", ortn_mag, 1.0);
+ n_.param("cartesian_x_axis", axis_x, 3);
+ n_.param("cartesian_y_axis", axis_y, 2);
+ n_.param("cartesian_z_axis", axis_z, 1);
+ n_.param("orientation_roll_axis", axis_r, 3);
+ n_.param("orientation_pitch_axis", axis_p, 2);
+ n_.param("orientation_yaw_axis", axis_yaw, 1);
+
+ hold.request.hold = false; // Default Start for joint hold command is false
+ cart_publish = ortn_publish = false; // Setting publisher states to false
+ bh_cmd_st = 0;// Initializing BarrettHand Command State to Zero
+
+ //Subscribers
+ joy_sub = n_.subscribe < sensor_msgs::Joy > ("joy", 1, &WamTeleop::joyCallback, this); // /joy
+
+ //Service Clients
+ grasp_vel_srv = nh_.serviceClient<wam_srvs::BHandGraspVel>("grasp_vel"); // /bhand/grasp_vel
+ spread_vel_srv = nh_.serviceClient<wam_srvs::BHandSpreadVel>("spread_vel"); // /bhand/spread_vel
+ go_home_srv = nw_.serviceClient<std_srvs::Empty>("go_home"); // /wam/go_home
+ hold_srv = nw_.serviceClient<wam_srvs::Hold>("hold_joint_pos"); // /wam/hold_joint_pos
+
+ //Publishers
+ cart_vel_pub = nw_.advertise<wam_msgs::RTCartVel>("cart_vel_cmd", 1); // /wam/cart_vel_cmd
+ ortn_vel_pub = nw_.advertise<wam_msgs::RTOrtnVel>("ortn_vel_cmd", 1); // /wam/ortn_vel_cmd
+}
+
+void WamTeleop::joyCallback(const sensor_msgs::Joy::ConstPtr& joy_msg)
+{
+ //Set our publishing states back to false for new commands
+ grsp_publish = sprd_publish = cart_publish = ortn_publish = home_publish = hold_publish = ortn_mode = false;
+
+ //Return with no deadman pressed or without first pose published yet.
+ if (!joy_msg->buttons[deadman_btn] | joy_msg->buttons[guardian_deadman_btn])
+ return;
+
+ if (joy_msg->buttons[ortn_btn]) //Checking our Orientation button state
+ ortn_mode = true;
+
+ // Gripper Grasp Command
+ if(joy_msg->buttons[gpr_open_btn]) //Checking to see if gripper open button is being pressed
+ {
+ bh_grsp_vel = -max_grsp_vel; // Set the velocity for the gripper command
+ grsp_publish = true; // set grasp publish state
+ }
+ else if(joy_msg->buttons[gpr_close_btn]) //Checking to see if gripper close button is being pressed
+ {
+ bh_grsp_vel = max_grsp_vel; // Set the velocity for the gripper command
+ grsp_publish = true; // set grasp publish state
+ }
+
+ // Gripper Spread Command
+ if (joy_msg->buttons[sprd_open_btn])
+ {
+ bh_sprd_vel = -max_sprd_vel;
+ sprd_publish = true;
+ }
+ else if (joy_msg->buttons[sprd_close_btn])
+ {
+ bh_sprd_vel = max_sprd_vel;
+ sprd_publish = true;
+ }
+
+ // Go Home Command
+ if (joy_msg->buttons[home_btn] && !home_st) // Checking to see if go home button is pressed and if it was pressed last callback loop
+ home_publish = true; // set true only when button is first pressed down
+ home_st = joy_msg->buttons[home_btn]; // setting the state for the next loop
+
+ // Hold Joints Command
+ if (joy_msg->buttons[hold_btn] && !hold_st)// Checking to see if hold position button is pressed and if it was pressed last callback loop
+ hold_publish = true;// set true only when button is first pressed down
+ hold_st = joy_msg->buttons[hold_btn];// setting the state for the next callback loop
+
+
+ //Cartesian Velocity Portion
+ if ((joy_msg->axes[axis_x] > 0.25 || joy_msg->axes[axis_x] < -0.25) && !ortn_mode)
+ {
+ req_xdir = joy_msg->axes[axis_x];
+ cart_publish = true;
+ }
+ else
+ req_xdir = 0.0;
+ if ((joy_msg->axes[axis_y] > 0.25 || joy_msg->axes[axis_y] < -0.25) && !ortn_mode)
+ {
+ req_ydir = joy_msg->axes[axis_y];
+ cart_publish = true;
+ }
+ else
+ req_ydir = 0.0;
+ if ((joy_msg->axes[axis_z] > 0.25 || joy_msg->axes[axis_z] < -0.25) && !ortn_mode)
+ {
+ req_zdir = joy_msg->axes[axis_z];
+ cart_publish = true;
+ }
+ else
+ req_zdir = 0.0;
+
+ //RPY Velocity Portion
+ if ((joy_msg->axes[axis_r] > 0.25 || joy_msg->axes[axis_r] < -0.25) && ortn_mode)
+ {
+ req_rdir = -joy_msg->axes[axis_r];
+ ortn_publish = true;
+ }
+ else
+ req_rdir = 0.0;
+ if ((joy_msg->axes[axis_p] > 0.25 || joy_msg->axes[axis_p] < -0.25) && ortn_mode)
+ {
+ req_pdir = -joy_msg->axes[axis_p];
+ ortn_publish = true;
+ }
+ else
+ req_pdir = 0.0;
+ if ((joy_msg->axes[axis_yaw] > 0.25 || joy_msg->axes[axis_yaw] < -0.25) && ortn_mode)
+ {
+ req_yawdir = joy_msg->axes[axis_yaw];
+ ortn_publish = true;
+ }
+ else
+ req_yawdir = 0.0;
+
+}
+
+// Function for updating the commands and publishing
+void WamTeleop::update()
+{
+ //Check our publish hand states and act accordingly
+ if (grsp_publish && bh_cmd_st == 0 && !sprd_publish && !cart_publish && !ortn_publish) // Only if grasp publish state is set
+ {
+ grasp_vel.request.velocity = bh_grsp_vel; // set grasp velocity to commanded
+ grasp_vel_srv.call(grasp_vel); // call grasp velocity service
+ bh_cmd_st = 1; // set the BarrettHand commanded state to signify grasp command
+ }
+ else if (sprd_publish && bh_cmd_st == 0 && !grsp_publish && !cart_publish && !ortn_publish) // only if spread publish state is set
+ {
+ spread_vel.request.velocity = bh_sprd_vel; // set spread velocity to commanded
+ spread_vel_srv.call(spread_vel); // call spread velocity service
+ bh_cmd_st = 2; // set the BarrettHand commanded state to signify spread command
+ }
+ else if (bh_cmd_st != 0 && !grsp_publish && !sprd_publish && !cart_publish && !ortn_publish) // only if nothing published and last bhand state !=0
+ {
+ if (bh_cmd_st == 1) // if BarrettHand state is in grasp mode
+ {
+ grasp_vel.request.velocity = 0.0; // Zero the velocity
+ grasp_vel_srv.call(grasp_vel); // Command zero velocity to grasp
+ }
+ if (bh_cmd_st == 2) // if Barretthand state is in spread mode
+ {
+ spread_vel.request.velocity = 0.0; // Zero the velocity
+ spread_vel_srv.call(spread_vel); // Command zero velocity to spread
+ }
+ bh_cmd_st = 0; // set the BarrettHand state to no mode (0)
+ }
+
+ //Check our publish hold joint position state and act accordingly
+ if (hold_publish && !cart_publish && !ortn_publish && !grsp_publish && !sprd_publish && !home_publish) // if only hold_publish state is set
+ {
+ if (!hold.request.hold) // Check previous holding state
+ hold.request.hold = true; // Setting the hold request to true;
+ else
+ hold.request.hold = false; // Setting the hold request to false
+ hold_srv.call(hold); // Call the commanded hold state
+ }
+
+ //Check our publish go home state and act accordingly
+ if(home_publish && !hold_publish && !cart_publish && !grsp_publish && !sprd_publish && !ortn_publish) // if only home_publish state is set
+ go_home_srv.call(go_home); // Command WAM to go home
+
+ //Check our published cartesian velocity state and act accordingly
+ if(cart_publish && !ortn_publish && !grsp_publish && !sprd_publish && !home_publish && !hold_publish) // if only cart_publish state is set
+ {
+ cart_vel.direction[0] = req_xdir;
+ cart_vel.direction[1] = req_ydir;
+ cart_vel.direction[2] = req_zdir;
+ cart_vel.magnitude = cart_mag;
+ cart_vel_pub.publish(cart_vel);
+ }
+
+ //Check our published orientation velocity state and act accordingly
+ if(ortn_publish && !cart_publish && !grsp_publish && !sprd_publish && !home_publish && !hold_publish) // if only cart_publish state is set
+ {
+ ortn_vel.angular[0] = req_rdir;
+ ortn_vel.angular[1] = req_pdir;
+ ortn_vel.angular[2] = req_yawdir;
+ ortn_vel.magnitude = ortn_mag;
+ ortn_vel_pub.publish(ortn_vel);
+ }
+}
+
+int main(int argc, char** argv)
+{
+ ros::init(argc, argv, "wam_teleop"); // Start our wam_node and name it "wam_teleop"
+ WamTeleop wam_teleop; // Declare a member of WamTeleop "wam_teleop"
+ wam_teleop.init(); // Initialize our teleoperation
+
+ ros::Rate pub_rate(CNTRL_FREQ); // Setting the publishing rate to CNTRL_FREQ (50Hz by default)
+
+ while (wam_teleop.n_.ok()) // Looping at the specified frequency while our ros node is ok
+ {
+ ros::spinOnce();
+ wam_teleop.update(); // Update and send commands to the WAM
+ pub_rate.sleep();
+ }
+ return 0;
+}
projects / barrett-ros-pkg.git / commitdiff