From: Walter Fetter Lages Date: Thu, 29 Nov 2018 04:59:59 +0000 (-0200) Subject: Initial port from original Fuerte version. X-Git-Url: http://git.ece.ufrgs.br/?a=commitdiff_plain;h=b3a5663b46ef5fefee7f832a4ed0601b4bc4a521;p=barrett-ros-pkg.git Initial port from original Fuerte version. --- b3a5663b46ef5fefee7f832a4ed0601b4bc4a521 diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..c1edc29 --- /dev/null +++ b/.gitignore @@ -0,0 +1,51 @@ +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 diff --git a/README-indigo.txt b/README-indigo.txt new file mode 100644 index 0000000..458b603 --- /dev/null +++ b/README-indigo.txt @@ -0,0 +1,8 @@ +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. + diff --git a/README.txt b/README.txt new file mode 100644 index 0000000..5f5ebbc --- /dev/null +++ b/README.txt @@ -0,0 +1,39 @@ +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 diff --git a/wam_common/CMakeLists.txt b/wam_common/CMakeLists.txt new file mode 100644 index 0000000..a252bd5 --- /dev/null +++ b/wam_common/CMakeLists.txt @@ -0,0 +1,4 @@ +cmake_minimum_required(VERSION 2.8.3) +project(wam_common) +find_package(catkin REQUIRED) +catkin_metapackage() diff --git a/wam_common/package.xml b/wam_common/package.xml new file mode 100644 index 0000000..cfd4791 --- /dev/null +++ b/wam_common/package.xml @@ -0,0 +1,57 @@ + + + wam_common + 2.0.0 + 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. + + + + + + Walter Fetter Lages + + + + + + BSD + + + + + + + + + + + + + Maintained by Barrett Techonlogy Inc., Kyle Maroney + + + + + + + + + + + + + catkin + wam_msgs + wam_srvs + wam_teleop + + + + + + + + diff --git a/wam_msgs/CMakeLists.txt b/wam_msgs/CMakeLists.txt new file mode 100644 index 0000000..909d64c --- /dev/null +++ b/wam_msgs/CMakeLists.txt @@ -0,0 +1,204 @@ +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) diff --git a/wam_msgs/msg/RTCartPos.msg b/wam_msgs/msg/RTCartPos.msg new file mode 100644 index 0000000..8cedc12 --- /dev/null +++ b/wam_msgs/msg/RTCartPos.msg @@ -0,0 +1,2 @@ +float32[3] position +float32[3] rate_limits diff --git a/wam_msgs/msg/RTCartVel.msg b/wam_msgs/msg/RTCartVel.msg new file mode 100644 index 0000000..7bd7e8a --- /dev/null +++ b/wam_msgs/msg/RTCartVel.msg @@ -0,0 +1,2 @@ +float32[3] direction +float32 magnitude diff --git a/wam_msgs/msg/RTJointPos.msg b/wam_msgs/msg/RTJointPos.msg new file mode 100644 index 0000000..329ee4f --- /dev/null +++ b/wam_msgs/msg/RTJointPos.msg @@ -0,0 +1,2 @@ +float32[] joints +float32[] rate_limits diff --git a/wam_msgs/msg/RTJointVel.msg b/wam_msgs/msg/RTJointVel.msg new file mode 100644 index 0000000..c18b070 --- /dev/null +++ b/wam_msgs/msg/RTJointVel.msg @@ -0,0 +1 @@ +float32[] velocities diff --git a/wam_msgs/msg/RTOrtnPos.msg b/wam_msgs/msg/RTOrtnPos.msg new file mode 100644 index 0000000..4d8369b --- /dev/null +++ b/wam_msgs/msg/RTOrtnPos.msg @@ -0,0 +1,2 @@ +float32[4] orientation +float32[4] rate_limits diff --git a/wam_msgs/msg/RTOrtnVel.msg b/wam_msgs/msg/RTOrtnVel.msg new file mode 100644 index 0000000..d8ebf88 --- /dev/null +++ b/wam_msgs/msg/RTOrtnVel.msg @@ -0,0 +1,2 @@ +float32[3] angular +float32 magnitude diff --git a/wam_msgs/msg/RTPose.msg b/wam_msgs/msg/RTPose.msg new file mode 100644 index 0000000..dabd7cd --- /dev/null +++ b/wam_msgs/msg/RTPose.msg @@ -0,0 +1,4 @@ +float32[3] position +float32[4] orientation +float32[3] pos_rate_limits +float32[4] ortn_rate_limits diff --git a/wam_msgs/package.xml b/wam_msgs/package.xml new file mode 100644 index 0000000..72e320d --- /dev/null +++ b/wam_msgs/package.xml @@ -0,0 +1,56 @@ + + + wam_msgs + 2.0.0 + The wam_msgss package + This Package contains WAM / BarrettHand specific messages. + Ported from the ROS Fuerte version to Indigo by Walter Fetter Lages. + + + + + + Walter Fetter Lages + + + + + + BSD + + + + + + + + + + + + + Barrett Technology Inc., Kyle Maroney + + + + + + + + + + + + + + catkin + message_generation + + message_runtime + + + + + + + diff --git a/wam_node/CMakeLists.txt b/wam_node/CMakeLists.txt new file mode 100644 index 0000000..d40c8d2 --- /dev/null +++ b/wam_node/CMakeLists.txt @@ -0,0 +1,204 @@ +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) diff --git a/wam_node/launch/wam_node.launch b/wam_node/launch/wam_node.launch new file mode 100644 index 0000000..3a14334 --- /dev/null +++ b/wam_node/launch/wam_node.launch @@ -0,0 +1,4 @@ + + + + diff --git a/wam_node/package.xml b/wam_node/package.xml new file mode 100644 index 0000000..ef52d82 --- /dev/null +++ b/wam_node/package.xml @@ -0,0 +1,72 @@ + + + wam_node + 2.0.0 + 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. + + + + + + Walter Fetter Lages + + + + + + GPL + + + + + + + http://ros.org/wiki/wam_node + + + + + + + Barrett Technology Inc., Kyle Maroney + + + + + + + + + + + + + + catkin + geometry_msgs + roscpp + sensor_msgs + std_msgs + std_srvs + tf + wam_msgs + wam_srvs + geometry_msgs + roscpp + sensor_msgs + std_msgs + std_srvs + tf + wam_msgs + wam_srvs + + + + + + + + diff --git a/wam_node/src/wam_node.cpp b/wam_node/src/wam_node.cpp new file mode 100644 index 0000000..866ca0b --- /dev/null +++ b/wam_node/src/wam_node.cpp @@ -0,0 +1,908 @@ +/* + Copyright 2012 Barrett Technology + + 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 + . + + 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 +#include + +#include // BarrettHand threading +#include + +#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 +#include +#include +#include +#include +#include +#include + +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 + class Multiplier : public systems::System, public systems::SingleOutput + { + public: + Input input1; + public: + Input input2; + + public: + Multiplier(std::string sysName = "Multiplier") : + systems::System(sysName), systems::SingleOutput(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::type, Eigen::Quaterniond> +{ +public: + Eigen::Quaterniond outputQuat; + +public: + ToQuaternion(std::string sysName = "ToQuaternion") : + systems::SingleIO::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 + 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& 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 orientationSetPoint, current_ortn; + systems::ExposedOutput cart_dir, current_cart_pos, cp_track; + systems::ExposedOutput::type> rpy_cmd, current_rpy_ortn; + systems::ExposedOutput jv_track; + systems::ExposedOutput jp_track; + systems::TupleGrouper rt_pose_cmd; + systems::Summer cart_pos_sum; + systems::Summer::type> ortn_cmd_sum; + systems::Ramp ramp; + systems::RateLimiter jp_rl; + systems::RateLimiter cp_rl; + Multiplier mult_linear; + Multiplier::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& 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 + void WamNode::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::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 + bool WamNode::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 + bool WamNode::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 + bool WamNode::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 + bool WamNode::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 + bool WamNode::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 + bool WamNode::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 + bool WamNode::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 + bool WamNode::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 + bool WamNode::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 + bool WamNode::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 + bool WamNode::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 + bool WamNode::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 + bool WamNode::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 + bool WamNode::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 + bool WamNode::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 + bool WamNode::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 + bool WamNode::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 + bool WamNode::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 + bool WamNode::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 + void WamNode::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 + void WamNode::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 + void WamNode::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 + void WamNode::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 + void WamNode::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 + void WamNode::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 + void WamNode::publishHand() //systems::PeriodicDataLogger& 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 + void WamNode::updateRT(ProductManager& pm) //systems::PeriodicDataLogger& 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 + int wam_main(int argc, char** argv, ProductManager& pm, systems::Wam& wam) + { + BARRETT_UNITS_TEMPLATE_TYPEDEFS(DOF); + ros::init(argc, argv, "wam_node"); + WamNode wam_node(wam); + wam_node.init(pm); + ros::Rate pub_rate(PUBLISH_FREQ); + + if (pm.getHand()) + boost::thread handPubThread(&WamNode::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; + } diff --git a/wam_robot/CMakeLists.txt b/wam_robot/CMakeLists.txt new file mode 100644 index 0000000..286d86e --- /dev/null +++ b/wam_robot/CMakeLists.txt @@ -0,0 +1,4 @@ +cmake_minimum_required(VERSION 2.8.3) +project(wam_robot) +find_package(catkin REQUIRED) +catkin_metapackage() diff --git a/wam_robot/package.xml b/wam_robot/package.xml new file mode 100644 index 0000000..4e335fd --- /dev/null +++ b/wam_robot/package.xml @@ -0,0 +1,58 @@ + + + wam_robot + 2.0.0 + 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. + + + + + + Walter Fetter Lages + + + + + + GPL + + + + + + + http://ros.org/wiki/wam_robot + + + + + + Barrett Technology Inc., Kyle Maroney + + + + + + + + + + + + + catkin + wam_node + wam_srvs + wam_teleop + + + + + + + + diff --git a/wam_srvs/CMakeLists.txt b/wam_srvs/CMakeLists.txt new file mode 100644 index 0000000..dda37b2 --- /dev/null +++ b/wam_srvs/CMakeLists.txt @@ -0,0 +1,210 @@ +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) diff --git a/wam_srvs/package.xml b/wam_srvs/package.xml new file mode 100644 index 0000000..2137a16 --- /dev/null +++ b/wam_srvs/package.xml @@ -0,0 +1,57 @@ + + + wam_srvs + 0.0.0 + The wam_srvs package + This Package contains WAM / BarrettHand specific service definitions. + Ported from the ROS Fuerte version to Indigo by Walter Fetter Lages. + + + + + + Walter Fetter Lages + + + + + + BSD + + + + + + + + + + + + + Barrett Technology Inc., Kyle Maroney + + + + + + + + + + + + + catkin + geometry_msgs + message_generation + geometry_msgs + message_runtime + + + + + + + + diff --git a/wam_srvs/srv/BHandFingerPos.srv b/wam_srvs/srv/BHandFingerPos.srv new file mode 100644 index 0000000..581e715 --- /dev/null +++ b/wam_srvs/srv/BHandFingerPos.srv @@ -0,0 +1,2 @@ +float32[3] radians +--- diff --git a/wam_srvs/srv/BHandFingerVel.srv b/wam_srvs/srv/BHandFingerVel.srv new file mode 100644 index 0000000..535ff26 --- /dev/null +++ b/wam_srvs/srv/BHandFingerVel.srv @@ -0,0 +1,2 @@ +float32[3] velocity +--- diff --git a/wam_srvs/srv/BHandGraspPos.srv b/wam_srvs/srv/BHandGraspPos.srv new file mode 100644 index 0000000..7f42d47 --- /dev/null +++ b/wam_srvs/srv/BHandGraspPos.srv @@ -0,0 +1,2 @@ +float32 radians +--- diff --git a/wam_srvs/srv/BHandGraspVel.srv b/wam_srvs/srv/BHandGraspVel.srv new file mode 100644 index 0000000..90356a2 --- /dev/null +++ b/wam_srvs/srv/BHandGraspVel.srv @@ -0,0 +1,2 @@ +float32 velocity +--- diff --git a/wam_srvs/srv/BHandSpreadPos.srv b/wam_srvs/srv/BHandSpreadPos.srv new file mode 100644 index 0000000..7f42d47 --- /dev/null +++ b/wam_srvs/srv/BHandSpreadPos.srv @@ -0,0 +1,2 @@ +float32 radians +--- diff --git a/wam_srvs/srv/BHandSpreadVel.srv b/wam_srvs/srv/BHandSpreadVel.srv new file mode 100644 index 0000000..90356a2 --- /dev/null +++ b/wam_srvs/srv/BHandSpreadVel.srv @@ -0,0 +1,2 @@ +float32 velocity +--- diff --git a/wam_srvs/srv/CartPosMove.srv b/wam_srvs/srv/CartPosMove.srv new file mode 100644 index 0000000..01563df --- /dev/null +++ b/wam_srvs/srv/CartPosMove.srv @@ -0,0 +1,2 @@ +float32[3] position +--- \ No newline at end of file diff --git a/wam_srvs/srv/GravityComp.srv b/wam_srvs/srv/GravityComp.srv new file mode 100644 index 0000000..102ea81 --- /dev/null +++ b/wam_srvs/srv/GravityComp.srv @@ -0,0 +1,2 @@ +bool gravity +--- diff --git a/wam_srvs/srv/Hold.srv b/wam_srvs/srv/Hold.srv new file mode 100644 index 0000000..1ad9ec1 --- /dev/null +++ b/wam_srvs/srv/Hold.srv @@ -0,0 +1,2 @@ +bool hold +--- diff --git a/wam_srvs/srv/JointMove.srv b/wam_srvs/srv/JointMove.srv new file mode 100644 index 0000000..0197e85 --- /dev/null +++ b/wam_srvs/srv/JointMove.srv @@ -0,0 +1,2 @@ +float32[] joints +--- diff --git a/wam_srvs/srv/OrtnMove.srv b/wam_srvs/srv/OrtnMove.srv new file mode 100644 index 0000000..e7c30c0 --- /dev/null +++ b/wam_srvs/srv/OrtnMove.srv @@ -0,0 +1,2 @@ +float32[4] orientation +--- \ No newline at end of file diff --git a/wam_srvs/srv/PoseMove.srv b/wam_srvs/srv/PoseMove.srv new file mode 100644 index 0000000..66f26e8 --- /dev/null +++ b/wam_srvs/srv/PoseMove.srv @@ -0,0 +1,2 @@ +geometry_msgs/Pose pose +--- diff --git a/wam_teleop/CMakeLists.txt b/wam_teleop/CMakeLists.txt new file mode 100644 index 0000000..ab496bc --- /dev/null +++ b/wam_teleop/CMakeLists.txt @@ -0,0 +1,202 @@ +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) diff --git a/wam_teleop/launch/wam_joystick_teleop.launch b/wam_teleop/launch/wam_joystick_teleop.launch new file mode 100644 index 0000000..ff1fbb1 --- /dev/null +++ b/wam_teleop/launch/wam_joystick_teleop.launch @@ -0,0 +1,12 @@ + + + + + + + + + + + + diff --git a/wam_teleop/launch/wam_wingman_teleop.launch b/wam_teleop/launch/wam_wingman_teleop.launch new file mode 100644 index 0000000..6d71321 --- /dev/null +++ b/wam_teleop/launch/wam_wingman_teleop.launch @@ -0,0 +1,36 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/wam_teleop/package.xml b/wam_teleop/package.xml new file mode 100644 index 0000000..5a6e852 --- /dev/null +++ b/wam_teleop/package.xml @@ -0,0 +1,66 @@ + + + wam_teleop + 2.0.0 + 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. + + + + + + Walter Fetter Lages + + + + + + BSD + + + + + + + + + + + + + Barrett Technology Inc., Kyle Maroney + + + + + + + + + + + + + catkin + joy + roscpp + sensor_msgs + std_srvs + wam_msgs + wam_srvs + joy + roscpp + sensor_msgs + std_srvs + wam_msgs + wam_srvs + + + + + + + + diff --git a/wam_teleop/src/wam_joystick_teleop.cpp b/wam_teleop/src/wam_joystick_teleop.cpp new file mode 100644 index 0000000..99bacae --- /dev/null +++ b/wam_teleop/src/wam_joystick_teleop.cpp @@ -0,0 +1,297 @@ +#include +#include + +#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("grasp_vel"); // /bhand/grasp_vel + spread_vel_srv = nh_.serviceClient("spread_vel"); // /bhand/spread_vel + go_home_srv = nw_.serviceClient("go_home"); // /wam/go_home + hold_srv = nw_.serviceClient("hold_joint_pos"); // /wam/hold_joint_pos + + //Publishers + cart_vel_pub = nw_.advertise("cart_vel_cmd", 1); // /wam/cart_vel_cmd + ortn_vel_pub = nw_.advertise("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; +}