From: Chris Woodall Date: Tue, 20 Nov 2018 21:00:47 +0000 (-0500) Subject: Addex experimental bh stuff X-Git-Url: http://git.ece.ufrgs.br/?a=commitdiff_plain;h=refs%2Fheads%2Fexperimental;p=barrett-ros-pkg.git Addex experimental bh stuff --- diff --git a/bh262_node/CMakeLists.txt b/bh262_node/CMakeLists.txt new file mode 100644 index 0000000..3d6646c --- /dev/null +++ b/bh262_node/CMakeLists.txt @@ -0,0 +1,19 @@ +cmake_minimum_required(VERSION 2.4.6) +include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake) + +# Set the build type. Options are: +# Coverage : w/ debug symbols, w/o optimization, w/ code-coverage +# Debug : w/ debug symbols, w/o optimization +# Release : w/o debug symbols, w/ optimization +# RelWithDebInfo : w/ debug symbols, w/ optimization +# MinSizeRel : w/o debug symbols, w/ optimization, stripped binaries +#set(ROS_BUILD_TYPE RelWithDebInfo) + +rosbuild_init() + +set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin) + +rosbuild_add_boost_directories() +rosbuild_add_executable(bh262_node src/bh262_node.cpp) +rosbuild_link_boost(bh262_node thread system) + diff --git a/bh262_node/Makefile b/bh262_node/Makefile new file mode 100644 index 0000000..f1d2c5d --- /dev/null +++ b/bh262_node/Makefile @@ -0,0 +1,361 @@ +# CMAKE generated file: DO NOT EDIT! +# Generated by "Unix Makefiles" Generator, CMake Version 2.8 + +# Default target executed when no arguments are given to make. +default_target: all +.PHONY : default_target + +#============================================================================= +# Special targets provided by cmake. + +# Disable implicit rules so canonical targets will work. +.SUFFIXES: + +# Remove some rules from gmake that .SUFFIXES does not remove. +SUFFIXES = + +.SUFFIXES: .hpux_make_needs_suffix_list + +# Suppress display of executed commands. +$(VERBOSE).SILENT: + +# A target that is always out of date. +cmake_force: +.PHONY : cmake_force + +#============================================================================= +# Set environment variables for the build. + +# The shell in which to execute make rules. +SHELL = /bin/sh + +# The CMake executable. +CMAKE_COMMAND = /usr/bin/cmake + +# The command to remove a file. +RM = /usr/bin/cmake -E remove -f + +# The top-level source directory on which CMake was run. +CMAKE_SOURCE_DIR = /home/robot/ros/barrett-ros-pkg/branches/experimental/bh262_node + +# The top-level build directory on which CMake was run. +CMAKE_BINARY_DIR = /home/robot/ros/barrett-ros-pkg/branches/experimental/bh262_node + +#============================================================================= +# Targets provided globally by CMake. + +# Special rule for the target edit_cache +edit_cache: + @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --cyan "Running interactive CMake command-line interface..." + /usr/bin/cmake -i . +.PHONY : edit_cache + +# Special rule for the target edit_cache +edit_cache/fast: edit_cache +.PHONY : edit_cache/fast + +# Special rule for the target rebuild_cache +rebuild_cache: + @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --cyan "Running CMake to regenerate build system..." + /usr/bin/cmake -H$(CMAKE_SOURCE_DIR) -B$(CMAKE_BINARY_DIR) +.PHONY : rebuild_cache + +# Special rule for the target rebuild_cache +rebuild_cache/fast: rebuild_cache +.PHONY : rebuild_cache/fast + +# The main all target +all: cmake_check_build_system + $(CMAKE_COMMAND) -E cmake_progress_start /home/robot/ros/barrett-ros-pkg/branches/experimental/bh262_node/CMakeFiles /home/robot/ros/barrett-ros-pkg/branches/experimental/bh262_node/CMakeFiles/progress.marks + $(MAKE) -f CMakeFiles/Makefile2 all + $(CMAKE_COMMAND) -E cmake_progress_start /home/robot/ros/barrett-ros-pkg/branches/experimental/bh262_node/CMakeFiles 0 +.PHONY : all + +# The main clean target +clean: + $(MAKE) -f CMakeFiles/Makefile2 clean +.PHONY : clean + +# The main clean target +clean/fast: clean +.PHONY : clean/fast + +# Prepare targets for installation. +preinstall: all + $(MAKE) -f CMakeFiles/Makefile2 preinstall +.PHONY : preinstall + +# Prepare targets for installation. +preinstall/fast: + $(MAKE) -f CMakeFiles/Makefile2 preinstall +.PHONY : preinstall/fast + +# clear depends +depend: + $(CMAKE_COMMAND) -H$(CMAKE_SOURCE_DIR) -B$(CMAKE_BINARY_DIR) --check-build-system CMakeFiles/Makefile.cmake 1 +.PHONY : depend + +#============================================================================= +# Target rules for targets named ROSBUILD_genmsg_cpp + +# Build rule for target. +ROSBUILD_genmsg_cpp: cmake_check_build_system + $(MAKE) -f CMakeFiles/Makefile2 ROSBUILD_genmsg_cpp +.PHONY : ROSBUILD_genmsg_cpp + +# fast build rule for target. +ROSBUILD_genmsg_cpp/fast: + $(MAKE) -f CMakeFiles/ROSBUILD_genmsg_cpp.dir/build.make CMakeFiles/ROSBUILD_genmsg_cpp.dir/build +.PHONY : ROSBUILD_genmsg_cpp/fast + +#============================================================================= +# Target rules for targets named ROSBUILD_genmsg_lisp + +# Build rule for target. +ROSBUILD_genmsg_lisp: cmake_check_build_system + $(MAKE) -f CMakeFiles/Makefile2 ROSBUILD_genmsg_lisp +.PHONY : ROSBUILD_genmsg_lisp + +# fast build rule for target. +ROSBUILD_genmsg_lisp/fast: + $(MAKE) -f CMakeFiles/ROSBUILD_genmsg_lisp.dir/build.make CMakeFiles/ROSBUILD_genmsg_lisp.dir/build +.PHONY : ROSBUILD_genmsg_lisp/fast + +#============================================================================= +# Target rules for targets named ROSBUILD_gensrv_cpp + +# Build rule for target. +ROSBUILD_gensrv_cpp: cmake_check_build_system + $(MAKE) -f CMakeFiles/Makefile2 ROSBUILD_gensrv_cpp +.PHONY : ROSBUILD_gensrv_cpp + +# fast build rule for target. +ROSBUILD_gensrv_cpp/fast: + $(MAKE) -f CMakeFiles/ROSBUILD_gensrv_cpp.dir/build.make CMakeFiles/ROSBUILD_gensrv_cpp.dir/build +.PHONY : ROSBUILD_gensrv_cpp/fast + +#============================================================================= +# Target rules for targets named ROSBUILD_gensrv_lisp + +# Build rule for target. +ROSBUILD_gensrv_lisp: cmake_check_build_system + $(MAKE) -f CMakeFiles/Makefile2 ROSBUILD_gensrv_lisp +.PHONY : ROSBUILD_gensrv_lisp + +# fast build rule for target. +ROSBUILD_gensrv_lisp/fast: + $(MAKE) -f CMakeFiles/ROSBUILD_gensrv_lisp.dir/build.make CMakeFiles/ROSBUILD_gensrv_lisp.dir/build +.PHONY : ROSBUILD_gensrv_lisp/fast + +#============================================================================= +# Target rules for targets named bh262_node + +# Build rule for target. +bh262_node: cmake_check_build_system + $(MAKE) -f CMakeFiles/Makefile2 bh262_node +.PHONY : bh262_node + +# fast build rule for target. +bh262_node/fast: + $(MAKE) -f CMakeFiles/bh262_node.dir/build.make CMakeFiles/bh262_node.dir/build +.PHONY : bh262_node/fast + +#============================================================================= +# Target rules for targets named clean-test-results + +# Build rule for target. +clean-test-results: cmake_check_build_system + $(MAKE) -f CMakeFiles/Makefile2 clean-test-results +.PHONY : clean-test-results + +# fast build rule for target. +clean-test-results/fast: + $(MAKE) -f CMakeFiles/clean-test-results.dir/build.make CMakeFiles/clean-test-results.dir/build +.PHONY : clean-test-results/fast + +#============================================================================= +# Target rules for targets named rosbuild_precompile + +# Build rule for target. +rosbuild_precompile: cmake_check_build_system + $(MAKE) -f CMakeFiles/Makefile2 rosbuild_precompile +.PHONY : rosbuild_precompile + +# fast build rule for target. +rosbuild_precompile/fast: + $(MAKE) -f CMakeFiles/rosbuild_precompile.dir/build.make CMakeFiles/rosbuild_precompile.dir/build +.PHONY : rosbuild_precompile/fast + +#============================================================================= +# Target rules for targets named rosbuild_premsgsrvgen + +# Build rule for target. +rosbuild_premsgsrvgen: cmake_check_build_system + $(MAKE) -f CMakeFiles/Makefile2 rosbuild_premsgsrvgen +.PHONY : rosbuild_premsgsrvgen + +# fast build rule for target. +rosbuild_premsgsrvgen/fast: + $(MAKE) -f CMakeFiles/rosbuild_premsgsrvgen.dir/build.make CMakeFiles/rosbuild_premsgsrvgen.dir/build +.PHONY : rosbuild_premsgsrvgen/fast + +#============================================================================= +# Target rules for targets named rospack_genmsg + +# Build rule for target. +rospack_genmsg: cmake_check_build_system + $(MAKE) -f CMakeFiles/Makefile2 rospack_genmsg +.PHONY : rospack_genmsg + +# fast build rule for target. +rospack_genmsg/fast: + $(MAKE) -f CMakeFiles/rospack_genmsg.dir/build.make CMakeFiles/rospack_genmsg.dir/build +.PHONY : rospack_genmsg/fast + +#============================================================================= +# Target rules for targets named rospack_genmsg_libexe + +# Build rule for target. +rospack_genmsg_libexe: cmake_check_build_system + $(MAKE) -f CMakeFiles/Makefile2 rospack_genmsg_libexe +.PHONY : rospack_genmsg_libexe + +# fast build rule for target. +rospack_genmsg_libexe/fast: + $(MAKE) -f CMakeFiles/rospack_genmsg_libexe.dir/build.make CMakeFiles/rospack_genmsg_libexe.dir/build +.PHONY : rospack_genmsg_libexe/fast + +#============================================================================= +# Target rules for targets named rospack_gensrv + +# Build rule for target. +rospack_gensrv: cmake_check_build_system + $(MAKE) -f CMakeFiles/Makefile2 rospack_gensrv +.PHONY : rospack_gensrv + +# fast build rule for target. +rospack_gensrv/fast: + $(MAKE) -f CMakeFiles/rospack_gensrv.dir/build.make CMakeFiles/rospack_gensrv.dir/build +.PHONY : rospack_gensrv/fast + +#============================================================================= +# Target rules for targets named test + +# Build rule for target. +test: cmake_check_build_system + $(MAKE) -f CMakeFiles/Makefile2 test +.PHONY : test + +# fast build rule for target. +test/fast: + $(MAKE) -f CMakeFiles/test.dir/build.make CMakeFiles/test.dir/build +.PHONY : test/fast + +#============================================================================= +# Target rules for targets named test-future + +# Build rule for target. +test-future: cmake_check_build_system + $(MAKE) -f CMakeFiles/Makefile2 test-future +.PHONY : test-future + +# fast build rule for target. +test-future/fast: + $(MAKE) -f CMakeFiles/test-future.dir/build.make CMakeFiles/test-future.dir/build +.PHONY : test-future/fast + +#============================================================================= +# Target rules for targets named test-results + +# Build rule for target. +test-results: cmake_check_build_system + $(MAKE) -f CMakeFiles/Makefile2 test-results +.PHONY : test-results + +# fast build rule for target. +test-results/fast: + $(MAKE) -f CMakeFiles/test-results.dir/build.make CMakeFiles/test-results.dir/build +.PHONY : test-results/fast + +#============================================================================= +# Target rules for targets named test-results-run + +# Build rule for target. +test-results-run: cmake_check_build_system + $(MAKE) -f CMakeFiles/Makefile2 test-results-run +.PHONY : test-results-run + +# fast build rule for target. +test-results-run/fast: + $(MAKE) -f CMakeFiles/test-results-run.dir/build.make CMakeFiles/test-results-run.dir/build +.PHONY : test-results-run/fast + +#============================================================================= +# Target rules for targets named tests + +# Build rule for target. +tests: cmake_check_build_system + $(MAKE) -f CMakeFiles/Makefile2 tests +.PHONY : tests + +# fast build rule for target. +tests/fast: + $(MAKE) -f CMakeFiles/tests.dir/build.make CMakeFiles/tests.dir/build +.PHONY : tests/fast + +# target to build an object file +src/bh262_node.o: + $(MAKE) -f CMakeFiles/bh262_node.dir/build.make CMakeFiles/bh262_node.dir/src/bh262_node.o +.PHONY : src/bh262_node.o + +# target to preprocess a source file +src/bh262_node.i: + $(MAKE) -f CMakeFiles/bh262_node.dir/build.make CMakeFiles/bh262_node.dir/src/bh262_node.i +.PHONY : src/bh262_node.i + +# target to generate assembly for a file +src/bh262_node.s: + $(MAKE) -f CMakeFiles/bh262_node.dir/build.make CMakeFiles/bh262_node.dir/src/bh262_node.s +.PHONY : src/bh262_node.s + +# Help Target +help: + @echo "The following are some of the valid targets for this Makefile:" + @echo "... all (the default if no target is provided)" + @echo "... clean" + @echo "... depend" + @echo "... ROSBUILD_genmsg_cpp" + @echo "... ROSBUILD_genmsg_lisp" + @echo "... ROSBUILD_gensrv_cpp" + @echo "... ROSBUILD_gensrv_lisp" + @echo "... bh262_node" + @echo "... clean-test-results" + @echo "... edit_cache" + @echo "... rebuild_cache" + @echo "... rosbuild_precompile" + @echo "... rosbuild_premsgsrvgen" + @echo "... rospack_genmsg" + @echo "... rospack_genmsg_libexe" + @echo "... rospack_gensrv" + @echo "... test" + @echo "... test-future" + @echo "... test-results" + @echo "... test-results-run" + @echo "... tests" + @echo "... src/bh262_node.o" + @echo "... src/bh262_node.i" + @echo "... src/bh262_node.s" +.PHONY : help + + + +#============================================================================= +# Special targets to cleanup operation of make. + +# Special rule to run CMake to check the build system integrity. +# No rule that depends on this can have commands that come from listfiles +# because they might be regenerated. +cmake_check_build_system: + $(CMAKE_COMMAND) -H$(CMAKE_SOURCE_DIR) -B$(CMAKE_BINARY_DIR) --check-build-system CMakeFiles/Makefile.cmake 0 +.PHONY : cmake_check_build_system + diff --git a/bh262_node/launch/bh262.launch b/bh262_node/launch/bh262.launch new file mode 100644 index 0000000..f9da925 --- /dev/null +++ b/bh262_node/launch/bh262.launch @@ -0,0 +1,4 @@ + + + + diff --git a/bh262_node/manifest.xml b/bh262_node/manifest.xml new file mode 100644 index 0000000..ffb690e --- /dev/null +++ b/bh262_node/manifest.xml @@ -0,0 +1,17 @@ + + + + This package allows for control of the BarrettHand 262 via serial communcation. + + + Kyle Maroney, Barrett Technology Inc. + BSD + + http://ros.org/wiki/bh262_node + + + + + + + diff --git a/bh262_node/src/bh262_node.cpp b/bh262_node/src/bh262_node.cpp new file mode 100644 index 0000000..a2feb7f --- /dev/null +++ b/bh262_node/src/bh262_node.cpp @@ -0,0 +1,293 @@ +/* + File: bh262_node.cpp + Date: 7 June, 2012 + Author: Kyle Maroney + */ + +#include +#include "ros/ros.h" + +//BHand Services / Joint Messages +#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" + +//Boost libraries +#include +#include +#include + +const std::string bhand_port = "/dev/ttyS0"; //Serial port location - can be found with command line $ dmesg | grep tty +const double pub_freq = 100; //Hz + +//BH262 Class +class BH262Node +{ +public: + ros::NodeHandle np_, nb_; + ros::Rate pub_rate; // Default 100Hz + std::string bhand_command; + + //Services to be made available + ros::ServiceServer hand_initialize_srv, 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; + + // Our serial port variables + boost::asio::serial_port_base::baud_rate baud; + boost::asio::serial_port_base::character_size char_size; + boost::asio::serial_port_base::flow_control flow; + boost::asio::serial_port_base::parity parity; + boost::asio::serial_port_base::stop_bits stopbits; + boost::asio::io_service io; + boost::asio::serial_port port; + + BH262Node() : + np_("~"), nb_("bhand"), pub_rate(pub_freq), baud(9600), char_size(8), flow( + boost::asio::serial_port_base::flow_control::none), parity(boost::asio::serial_port_base::parity::none), stopbits( + boost::asio::serial_port_base::stop_bits::one), port(io, bhand_port) + { + } + void + init(); + + void write(std::string buf) + { + //write command out to serial port + boost::asio::write(port, boost::asio::buffer(buf)); + boost::asio::write(port, boost::asio::buffer("\015", sizeof("\015") - 1)); + } + + ~BH262Node() + { + } + + bool + handInitialize(std_srvs::Empty::Request &req, std_srvs::Empty::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 + publish(); +}; + +void BH262Node::init() +{ + ROS_INFO("Openning BHand on serial port %s", bhand_port.c_str()); + + // Set the settings + port.set_option(baud); + port.set_option(char_size); + port.set_option(flow); + port.set_option(parity); + port.set_option(stopbits); + + //Advertise the following services only if there is a BarrettHand present + hand_initialize_srv = nb_.advertiseService("initialize", &BH262Node::handInitialize, this); // bhand/initialize + hand_open_grsp_srv = nb_.advertiseService("open_grasp", &BH262Node::handOpenGrasp, this); // bhand/open_grasp + hand_close_grsp_srv = nb_.advertiseService("close_grasp", &BH262Node::handCloseGrasp, this); // bhand/close_grasp + hand_open_sprd_srv = nb_.advertiseService("open_spread", &BH262Node::handOpenSpread, this); // bhand/open_spread + hand_close_sprd_srv = nb_.advertiseService("close_spread", &BH262Node::handCloseSpread, this); // bhand/close_spread + hand_fngr_pos_srv = nb_.advertiseService("finger_pos", &BH262Node::handFingerPos, this); // bhand/finger_pos + hand_grsp_pos_srv = nb_.advertiseService("grasp_pos", &BH262Node::handGraspPos, this); // bhand/grasp_pos + hand_sprd_pos_srv = nb_.advertiseService("spread_pos", &BH262Node::handSpreadPos, this); // bhand/spread_pos + hand_fngr_vel_srv = nb_.advertiseService("finger_vel", &BH262Node::handFingerVel, this); // bhand/finger_vel + hand_grsp_vel_srv = nb_.advertiseService("grasp_vel", &BH262Node::handGraspVel, this); // bhand/grasp_vel + hand_sprd_vel_srv = nb_.advertiseService("spread_vel", &BH262Node::handSpreadVel, this); // bhand/spread_vel + + ros::AsyncSpinner spinner(2); + spinner.start(); + +} + +bool BH262Node::handInitialize(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res) +{ + ROS_INFO("BHand Initialize Request"); + + // Write the command out to serial. + write("HI"); + return true; +} + +bool BH262Node::handOpenGrasp(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res) +{ + ROS_INFO("BHand Open Request"); + write("GO"); + return true; +} + +bool BH262Node::handCloseGrasp(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res) +{ + ROS_INFO("BHand Close Request"); + write("GC"); + return true; +} + +bool BH262Node::handOpenSpread(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res) +{ + ROS_INFO("Spread Open Request"); + write("SO"); + return true; +} + +bool BH262Node::handCloseSpread(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res) +{ + ROS_INFO("Spread Close Request"); + write("SC"); + return true; +} + +bool BH262Node::handFingerPos(wam_srvs::BHandFingerPos::Request &req, wam_srvs::BHandFingerPos::Response &res) +{ + ROS_INFO("Finger Position Request"); + bhand_command = "1M " + boost::lexical_cast(ceil(req.radians[0] * 5729.578)); + write(bhand_command); + bhand_command = "2M " + boost::lexical_cast(ceil(req.radians[1] * 5729.578)); + write(bhand_command); + bhand_command = "3M " + boost::lexical_cast(ceil(req.radians[2] * 5729.578)); + write(bhand_command); + return true; +} + +bool BH262Node::handGraspPos(wam_srvs::BHandGraspPos::Request &req, wam_srvs::BHandGraspPos::Response &res) +{ + ROS_INFO("Grasp Position Request"); + bhand_command = "GM " + boost::lexical_cast(ceil(req.radians * 5729.578)); + write(bhand_command); + return true; +} + +bool BH262Node::handSpreadPos(wam_srvs::BHandSpreadPos::Request &req, wam_srvs::BHandSpreadPos::Response &res) +{ + ROS_INFO("Spread Position Request"); + bhand_command = "SM " + boost::lexical_cast(ceil(req.radians * 1002.68)); + write(bhand_command); + return true; +} + +bool BH262Node::handFingerVel(wam_srvs::BHandFingerVel::Request &req, wam_srvs::BHandFingerVel::Response &res) +{ + ROS_INFO("Finger Velocities Request"); + + if (req.velocity[0] > 0 || req.velocity[1] > 0 || req.velocity[2] > 0) + { + bhand_command = "1FSET MCV " + boost::lexical_cast(ceil(abs(req.velocity[0]))); + write(bhand_command); + bhand_command = "2FSET MCV " + boost::lexical_cast(ceil(abs(req.velocity[1]))); + write(bhand_command); + bhand_command = "3FSET MCV " + boost::lexical_cast(ceil(abs(req.velocity[2]))); + write(bhand_command); + bhand_command = "GC"; + } + else + { + bhand_command = "1FSET MOV " + boost::lexical_cast(ceil(abs(req.velocity[0]))); + write(bhand_command); + bhand_command = "2FSET MOV " + boost::lexical_cast(ceil(abs(req.velocity[1]))); + write(bhand_command); + bhand_command = "3FSET MOV " + boost::lexical_cast(ceil(abs(req.velocity[2]))); + write(bhand_command); + bhand_command = "GO"; + } + write(bhand_command); + + bhand_command = "GFSET MCV 100 "; + write(bhand_command); + bhand_command = "GFSET MOV 100 "; + write(bhand_command); + return true; +} + +bool BH262Node::handGraspVel(wam_srvs::BHandGraspVel::Request &req, wam_srvs::BHandGraspVel::Response &res) +{ + if (req.velocity > 0.0) + { + bhand_command = "GFSET MCV " + boost::lexical_cast(ceil(abs(req.velocity))); + write(bhand_command); + bhand_command = "GC"; + } + else + { + bhand_command = "GFSET MOV " + boost::lexical_cast(ceil(abs(req.velocity))); + write(bhand_command); + bhand_command = "GO"; + } + write(bhand_command); + bhand_command = "GFSET MCV 100 "; + write(bhand_command); + bhand_command = "GFSET MOV 100 "; + write(bhand_command); + return true; +} + +bool BH262Node::handSpreadVel(wam_srvs::BHandSpreadVel::Request &req, wam_srvs::BHandSpreadVel::Response &res) +{ + if (req.velocity > 0.0) + { + bhand_command = "SFSET MCV " + boost::lexical_cast(ceil(abs(req.velocity))); + write(bhand_command); + bhand_command = "SC"; + } + else + { + bhand_command = "SFSET MOV " + boost::lexical_cast(ceil(abs(req.velocity))); + write(bhand_command); + bhand_command = "SO"; + } + write(bhand_command); + bhand_command = "GFSET MCV 100 "; + write(bhand_command); + bhand_command = "GFSET MOV 100 "; + write(bhand_command); + return true; +} + +void BH262Node::publish() +{ + //write("SFGET P"); + //write("GFGET P"); + //Working on reading these positions and converting them to joint_state messages +} + +int main(int argc, char **argv) +{ + ros::init(argc, argv, "bh262_node"); + + BH262Node bhand; + bhand.init(); + + ros::Rate pub_rate(10); + + while (ros::ok()) + { + ros::spinOnce(); + bhand.publish(); + pub_rate.sleep(); + } + + bhand.port.close(); + return 0; +} diff --git a/bh280_standalone/CMakeLists.txt b/bh280_standalone/CMakeLists.txt new file mode 100644 index 0000000..f53b4f9 --- /dev/null +++ b/bh280_standalone/CMakeLists.txt @@ -0,0 +1,25 @@ +cmake_minimum_required(VERSION 2.4.6) +include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake) + +# Set the build type. Options are: +# Coverage : w/ debug symbols, w/o optimization, w/ code-coverage +# Debug : w/ debug symbols, w/o optimization +# Release : w/o debug symbols, w/ optimization +# RelWithDebInfo : w/ debug symbols, w/ optimization +# MinSizeRel : w/o debug symbols, w/ optimization, stripped binaries +#set(ROS_BUILD_TYPE RelWithDebInfo) + +rosbuild_init() + +#set the default path for built executables to the "bin" directory +set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin) +#set the default path for built libraries to the "lib" directory +set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib) + +find_package(Barrett REQUIRED) +link_directories(${BARRETT_LIB_DIRS}) +include_directories(${BARRETT_INCLUDE_DIRS}) +add_definitions(${BARRETT_DEFINITIONS}) +rosbuild_add_executable(bh280_node src/bh280_node.cpp) +target_link_libraries(bh280_node ${BARRETT_LIBRARIES}) + diff --git a/bh280_standalone/Makefile b/bh280_standalone/Makefile new file mode 100644 index 0000000..6bf4038 --- /dev/null +++ b/bh280_standalone/Makefile @@ -0,0 +1,361 @@ +# CMAKE generated file: DO NOT EDIT! +# Generated by "Unix Makefiles" Generator, CMake Version 2.8 + +# Default target executed when no arguments are given to make. +default_target: all +.PHONY : default_target + +#============================================================================= +# Special targets provided by cmake. + +# Disable implicit rules so canonical targets will work. +.SUFFIXES: + +# Remove some rules from gmake that .SUFFIXES does not remove. +SUFFIXES = + +.SUFFIXES: .hpux_make_needs_suffix_list + +# Suppress display of executed commands. +$(VERBOSE).SILENT: + +# A target that is always out of date. +cmake_force: +.PHONY : cmake_force + +#============================================================================= +# Set environment variables for the build. + +# The shell in which to execute make rules. +SHELL = /bin/sh + +# The CMake executable. +CMAKE_COMMAND = /usr/bin/cmake + +# The command to remove a file. +RM = /usr/bin/cmake -E remove -f + +# The top-level source directory on which CMake was run. +CMAKE_SOURCE_DIR = /home/robot/ros/barrett-ros-pkg/branches/experimental/bh280_standalone + +# The top-level build directory on which CMake was run. +CMAKE_BINARY_DIR = /home/robot/ros/barrett-ros-pkg/branches/experimental/bh280_standalone + +#============================================================================= +# Targets provided globally by CMake. + +# Special rule for the target edit_cache +edit_cache: + @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --cyan "Running interactive CMake command-line interface..." + /usr/bin/cmake -i . +.PHONY : edit_cache + +# Special rule for the target edit_cache +edit_cache/fast: edit_cache +.PHONY : edit_cache/fast + +# Special rule for the target rebuild_cache +rebuild_cache: + @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --cyan "Running CMake to regenerate build system..." + /usr/bin/cmake -H$(CMAKE_SOURCE_DIR) -B$(CMAKE_BINARY_DIR) +.PHONY : rebuild_cache + +# Special rule for the target rebuild_cache +rebuild_cache/fast: rebuild_cache +.PHONY : rebuild_cache/fast + +# The main all target +all: cmake_check_build_system + $(CMAKE_COMMAND) -E cmake_progress_start /home/robot/ros/barrett-ros-pkg/branches/experimental/bh280_standalone/CMakeFiles /home/robot/ros/barrett-ros-pkg/branches/experimental/bh280_standalone/CMakeFiles/progress.marks + $(MAKE) -f CMakeFiles/Makefile2 all + $(CMAKE_COMMAND) -E cmake_progress_start /home/robot/ros/barrett-ros-pkg/branches/experimental/bh280_standalone/CMakeFiles 0 +.PHONY : all + +# The main clean target +clean: + $(MAKE) -f CMakeFiles/Makefile2 clean +.PHONY : clean + +# The main clean target +clean/fast: clean +.PHONY : clean/fast + +# Prepare targets for installation. +preinstall: all + $(MAKE) -f CMakeFiles/Makefile2 preinstall +.PHONY : preinstall + +# Prepare targets for installation. +preinstall/fast: + $(MAKE) -f CMakeFiles/Makefile2 preinstall +.PHONY : preinstall/fast + +# clear depends +depend: + $(CMAKE_COMMAND) -H$(CMAKE_SOURCE_DIR) -B$(CMAKE_BINARY_DIR) --check-build-system CMakeFiles/Makefile.cmake 1 +.PHONY : depend + +#============================================================================= +# Target rules for targets named ROSBUILD_genmsg_cpp + +# Build rule for target. +ROSBUILD_genmsg_cpp: cmake_check_build_system + $(MAKE) -f CMakeFiles/Makefile2 ROSBUILD_genmsg_cpp +.PHONY : ROSBUILD_genmsg_cpp + +# fast build rule for target. +ROSBUILD_genmsg_cpp/fast: + $(MAKE) -f CMakeFiles/ROSBUILD_genmsg_cpp.dir/build.make CMakeFiles/ROSBUILD_genmsg_cpp.dir/build +.PHONY : ROSBUILD_genmsg_cpp/fast + +#============================================================================= +# Target rules for targets named ROSBUILD_genmsg_lisp + +# Build rule for target. +ROSBUILD_genmsg_lisp: cmake_check_build_system + $(MAKE) -f CMakeFiles/Makefile2 ROSBUILD_genmsg_lisp +.PHONY : ROSBUILD_genmsg_lisp + +# fast build rule for target. +ROSBUILD_genmsg_lisp/fast: + $(MAKE) -f CMakeFiles/ROSBUILD_genmsg_lisp.dir/build.make CMakeFiles/ROSBUILD_genmsg_lisp.dir/build +.PHONY : ROSBUILD_genmsg_lisp/fast + +#============================================================================= +# Target rules for targets named ROSBUILD_gensrv_cpp + +# Build rule for target. +ROSBUILD_gensrv_cpp: cmake_check_build_system + $(MAKE) -f CMakeFiles/Makefile2 ROSBUILD_gensrv_cpp +.PHONY : ROSBUILD_gensrv_cpp + +# fast build rule for target. +ROSBUILD_gensrv_cpp/fast: + $(MAKE) -f CMakeFiles/ROSBUILD_gensrv_cpp.dir/build.make CMakeFiles/ROSBUILD_gensrv_cpp.dir/build +.PHONY : ROSBUILD_gensrv_cpp/fast + +#============================================================================= +# Target rules for targets named ROSBUILD_gensrv_lisp + +# Build rule for target. +ROSBUILD_gensrv_lisp: cmake_check_build_system + $(MAKE) -f CMakeFiles/Makefile2 ROSBUILD_gensrv_lisp +.PHONY : ROSBUILD_gensrv_lisp + +# fast build rule for target. +ROSBUILD_gensrv_lisp/fast: + $(MAKE) -f CMakeFiles/ROSBUILD_gensrv_lisp.dir/build.make CMakeFiles/ROSBUILD_gensrv_lisp.dir/build +.PHONY : ROSBUILD_gensrv_lisp/fast + +#============================================================================= +# Target rules for targets named bh280_node + +# Build rule for target. +bh280_node: cmake_check_build_system + $(MAKE) -f CMakeFiles/Makefile2 bh280_node +.PHONY : bh280_node + +# fast build rule for target. +bh280_node/fast: + $(MAKE) -f CMakeFiles/bh280_node.dir/build.make CMakeFiles/bh280_node.dir/build +.PHONY : bh280_node/fast + +#============================================================================= +# Target rules for targets named clean-test-results + +# Build rule for target. +clean-test-results: cmake_check_build_system + $(MAKE) -f CMakeFiles/Makefile2 clean-test-results +.PHONY : clean-test-results + +# fast build rule for target. +clean-test-results/fast: + $(MAKE) -f CMakeFiles/clean-test-results.dir/build.make CMakeFiles/clean-test-results.dir/build +.PHONY : clean-test-results/fast + +#============================================================================= +# Target rules for targets named rosbuild_precompile + +# Build rule for target. +rosbuild_precompile: cmake_check_build_system + $(MAKE) -f CMakeFiles/Makefile2 rosbuild_precompile +.PHONY : rosbuild_precompile + +# fast build rule for target. +rosbuild_precompile/fast: + $(MAKE) -f CMakeFiles/rosbuild_precompile.dir/build.make CMakeFiles/rosbuild_precompile.dir/build +.PHONY : rosbuild_precompile/fast + +#============================================================================= +# Target rules for targets named rosbuild_premsgsrvgen + +# Build rule for target. +rosbuild_premsgsrvgen: cmake_check_build_system + $(MAKE) -f CMakeFiles/Makefile2 rosbuild_premsgsrvgen +.PHONY : rosbuild_premsgsrvgen + +# fast build rule for target. +rosbuild_premsgsrvgen/fast: + $(MAKE) -f CMakeFiles/rosbuild_premsgsrvgen.dir/build.make CMakeFiles/rosbuild_premsgsrvgen.dir/build +.PHONY : rosbuild_premsgsrvgen/fast + +#============================================================================= +# Target rules for targets named rospack_genmsg + +# Build rule for target. +rospack_genmsg: cmake_check_build_system + $(MAKE) -f CMakeFiles/Makefile2 rospack_genmsg +.PHONY : rospack_genmsg + +# fast build rule for target. +rospack_genmsg/fast: + $(MAKE) -f CMakeFiles/rospack_genmsg.dir/build.make CMakeFiles/rospack_genmsg.dir/build +.PHONY : rospack_genmsg/fast + +#============================================================================= +# Target rules for targets named rospack_genmsg_libexe + +# Build rule for target. +rospack_genmsg_libexe: cmake_check_build_system + $(MAKE) -f CMakeFiles/Makefile2 rospack_genmsg_libexe +.PHONY : rospack_genmsg_libexe + +# fast build rule for target. +rospack_genmsg_libexe/fast: + $(MAKE) -f CMakeFiles/rospack_genmsg_libexe.dir/build.make CMakeFiles/rospack_genmsg_libexe.dir/build +.PHONY : rospack_genmsg_libexe/fast + +#============================================================================= +# Target rules for targets named rospack_gensrv + +# Build rule for target. +rospack_gensrv: cmake_check_build_system + $(MAKE) -f CMakeFiles/Makefile2 rospack_gensrv +.PHONY : rospack_gensrv + +# fast build rule for target. +rospack_gensrv/fast: + $(MAKE) -f CMakeFiles/rospack_gensrv.dir/build.make CMakeFiles/rospack_gensrv.dir/build +.PHONY : rospack_gensrv/fast + +#============================================================================= +# Target rules for targets named test + +# Build rule for target. +test: cmake_check_build_system + $(MAKE) -f CMakeFiles/Makefile2 test +.PHONY : test + +# fast build rule for target. +test/fast: + $(MAKE) -f CMakeFiles/test.dir/build.make CMakeFiles/test.dir/build +.PHONY : test/fast + +#============================================================================= +# Target rules for targets named test-future + +# Build rule for target. +test-future: cmake_check_build_system + $(MAKE) -f CMakeFiles/Makefile2 test-future +.PHONY : test-future + +# fast build rule for target. +test-future/fast: + $(MAKE) -f CMakeFiles/test-future.dir/build.make CMakeFiles/test-future.dir/build +.PHONY : test-future/fast + +#============================================================================= +# Target rules for targets named test-results + +# Build rule for target. +test-results: cmake_check_build_system + $(MAKE) -f CMakeFiles/Makefile2 test-results +.PHONY : test-results + +# fast build rule for target. +test-results/fast: + $(MAKE) -f CMakeFiles/test-results.dir/build.make CMakeFiles/test-results.dir/build +.PHONY : test-results/fast + +#============================================================================= +# Target rules for targets named test-results-run + +# Build rule for target. +test-results-run: cmake_check_build_system + $(MAKE) -f CMakeFiles/Makefile2 test-results-run +.PHONY : test-results-run + +# fast build rule for target. +test-results-run/fast: + $(MAKE) -f CMakeFiles/test-results-run.dir/build.make CMakeFiles/test-results-run.dir/build +.PHONY : test-results-run/fast + +#============================================================================= +# Target rules for targets named tests + +# Build rule for target. +tests: cmake_check_build_system + $(MAKE) -f CMakeFiles/Makefile2 tests +.PHONY : tests + +# fast build rule for target. +tests/fast: + $(MAKE) -f CMakeFiles/tests.dir/build.make CMakeFiles/tests.dir/build +.PHONY : tests/fast + +# target to build an object file +src/bh280_node.o: + $(MAKE) -f CMakeFiles/bh280_node.dir/build.make CMakeFiles/bh280_node.dir/src/bh280_node.o +.PHONY : src/bh280_node.o + +# target to preprocess a source file +src/bh280_node.i: + $(MAKE) -f CMakeFiles/bh280_node.dir/build.make CMakeFiles/bh280_node.dir/src/bh280_node.i +.PHONY : src/bh280_node.i + +# target to generate assembly for a file +src/bh280_node.s: + $(MAKE) -f CMakeFiles/bh280_node.dir/build.make CMakeFiles/bh280_node.dir/src/bh280_node.s +.PHONY : src/bh280_node.s + +# Help Target +help: + @echo "The following are some of the valid targets for this Makefile:" + @echo "... all (the default if no target is provided)" + @echo "... clean" + @echo "... depend" + @echo "... ROSBUILD_genmsg_cpp" + @echo "... ROSBUILD_genmsg_lisp" + @echo "... ROSBUILD_gensrv_cpp" + @echo "... ROSBUILD_gensrv_lisp" + @echo "... bh280_node" + @echo "... clean-test-results" + @echo "... edit_cache" + @echo "... rebuild_cache" + @echo "... rosbuild_precompile" + @echo "... rosbuild_premsgsrvgen" + @echo "... rospack_genmsg" + @echo "... rospack_genmsg_libexe" + @echo "... rospack_gensrv" + @echo "... test" + @echo "... test-future" + @echo "... test-results" + @echo "... test-results-run" + @echo "... tests" + @echo "... src/bh280_node.o" + @echo "... src/bh280_node.i" + @echo "... src/bh280_node.s" +.PHONY : help + + + +#============================================================================= +# Special targets to cleanup operation of make. + +# Special rule to run CMake to check the build system integrity. +# No rule that depends on this can have commands that come from listfiles +# because they might be regenerated. +cmake_check_build_system: + $(CMAKE_COMMAND) -H$(CMAKE_SOURCE_DIR) -B$(CMAKE_BINARY_DIR) --check-build-system CMakeFiles/Makefile.cmake 0 +.PHONY : cmake_check_build_system + diff --git a/bh280_standalone/bh262_node/CMakeLists.txt b/bh280_standalone/bh262_node/CMakeLists.txt new file mode 100644 index 0000000..3d6646c --- /dev/null +++ b/bh280_standalone/bh262_node/CMakeLists.txt @@ -0,0 +1,19 @@ +cmake_minimum_required(VERSION 2.4.6) +include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake) + +# Set the build type. Options are: +# Coverage : w/ debug symbols, w/o optimization, w/ code-coverage +# Debug : w/ debug symbols, w/o optimization +# Release : w/o debug symbols, w/ optimization +# RelWithDebInfo : w/ debug symbols, w/ optimization +# MinSizeRel : w/o debug symbols, w/ optimization, stripped binaries +#set(ROS_BUILD_TYPE RelWithDebInfo) + +rosbuild_init() + +set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin) + +rosbuild_add_boost_directories() +rosbuild_add_executable(bh262_node src/bh262_node.cpp) +rosbuild_link_boost(bh262_node thread system) + diff --git a/bh280_standalone/bh262_node/Makefile b/bh280_standalone/bh262_node/Makefile new file mode 100644 index 0000000..f1d2c5d --- /dev/null +++ b/bh280_standalone/bh262_node/Makefile @@ -0,0 +1,361 @@ +# CMAKE generated file: DO NOT EDIT! +# Generated by "Unix Makefiles" Generator, CMake Version 2.8 + +# Default target executed when no arguments are given to make. +default_target: all +.PHONY : default_target + +#============================================================================= +# Special targets provided by cmake. + +# Disable implicit rules so canonical targets will work. +.SUFFIXES: + +# Remove some rules from gmake that .SUFFIXES does not remove. +SUFFIXES = + +.SUFFIXES: .hpux_make_needs_suffix_list + +# Suppress display of executed commands. +$(VERBOSE).SILENT: + +# A target that is always out of date. +cmake_force: +.PHONY : cmake_force + +#============================================================================= +# Set environment variables for the build. + +# The shell in which to execute make rules. +SHELL = /bin/sh + +# The CMake executable. +CMAKE_COMMAND = /usr/bin/cmake + +# The command to remove a file. +RM = /usr/bin/cmake -E remove -f + +# The top-level source directory on which CMake was run. +CMAKE_SOURCE_DIR = /home/robot/ros/barrett-ros-pkg/branches/experimental/bh262_node + +# The top-level build directory on which CMake was run. +CMAKE_BINARY_DIR = /home/robot/ros/barrett-ros-pkg/branches/experimental/bh262_node + +#============================================================================= +# Targets provided globally by CMake. + +# Special rule for the target edit_cache +edit_cache: + @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --cyan "Running interactive CMake command-line interface..." + /usr/bin/cmake -i . +.PHONY : edit_cache + +# Special rule for the target edit_cache +edit_cache/fast: edit_cache +.PHONY : edit_cache/fast + +# Special rule for the target rebuild_cache +rebuild_cache: + @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --cyan "Running CMake to regenerate build system..." + /usr/bin/cmake -H$(CMAKE_SOURCE_DIR) -B$(CMAKE_BINARY_DIR) +.PHONY : rebuild_cache + +# Special rule for the target rebuild_cache +rebuild_cache/fast: rebuild_cache +.PHONY : rebuild_cache/fast + +# The main all target +all: cmake_check_build_system + $(CMAKE_COMMAND) -E cmake_progress_start /home/robot/ros/barrett-ros-pkg/branches/experimental/bh262_node/CMakeFiles /home/robot/ros/barrett-ros-pkg/branches/experimental/bh262_node/CMakeFiles/progress.marks + $(MAKE) -f CMakeFiles/Makefile2 all + $(CMAKE_COMMAND) -E cmake_progress_start /home/robot/ros/barrett-ros-pkg/branches/experimental/bh262_node/CMakeFiles 0 +.PHONY : all + +# The main clean target +clean: + $(MAKE) -f CMakeFiles/Makefile2 clean +.PHONY : clean + +# The main clean target +clean/fast: clean +.PHONY : clean/fast + +# Prepare targets for installation. +preinstall: all + $(MAKE) -f CMakeFiles/Makefile2 preinstall +.PHONY : preinstall + +# Prepare targets for installation. +preinstall/fast: + $(MAKE) -f CMakeFiles/Makefile2 preinstall +.PHONY : preinstall/fast + +# clear depends +depend: + $(CMAKE_COMMAND) -H$(CMAKE_SOURCE_DIR) -B$(CMAKE_BINARY_DIR) --check-build-system CMakeFiles/Makefile.cmake 1 +.PHONY : depend + +#============================================================================= +# Target rules for targets named ROSBUILD_genmsg_cpp + +# Build rule for target. +ROSBUILD_genmsg_cpp: cmake_check_build_system + $(MAKE) -f CMakeFiles/Makefile2 ROSBUILD_genmsg_cpp +.PHONY : ROSBUILD_genmsg_cpp + +# fast build rule for target. +ROSBUILD_genmsg_cpp/fast: + $(MAKE) -f CMakeFiles/ROSBUILD_genmsg_cpp.dir/build.make CMakeFiles/ROSBUILD_genmsg_cpp.dir/build +.PHONY : ROSBUILD_genmsg_cpp/fast + +#============================================================================= +# Target rules for targets named ROSBUILD_genmsg_lisp + +# Build rule for target. +ROSBUILD_genmsg_lisp: cmake_check_build_system + $(MAKE) -f CMakeFiles/Makefile2 ROSBUILD_genmsg_lisp +.PHONY : ROSBUILD_genmsg_lisp + +# fast build rule for target. +ROSBUILD_genmsg_lisp/fast: + $(MAKE) -f CMakeFiles/ROSBUILD_genmsg_lisp.dir/build.make CMakeFiles/ROSBUILD_genmsg_lisp.dir/build +.PHONY : ROSBUILD_genmsg_lisp/fast + +#============================================================================= +# Target rules for targets named ROSBUILD_gensrv_cpp + +# Build rule for target. +ROSBUILD_gensrv_cpp: cmake_check_build_system + $(MAKE) -f CMakeFiles/Makefile2 ROSBUILD_gensrv_cpp +.PHONY : ROSBUILD_gensrv_cpp + +# fast build rule for target. +ROSBUILD_gensrv_cpp/fast: + $(MAKE) -f CMakeFiles/ROSBUILD_gensrv_cpp.dir/build.make CMakeFiles/ROSBUILD_gensrv_cpp.dir/build +.PHONY : ROSBUILD_gensrv_cpp/fast + +#============================================================================= +# Target rules for targets named ROSBUILD_gensrv_lisp + +# Build rule for target. +ROSBUILD_gensrv_lisp: cmake_check_build_system + $(MAKE) -f CMakeFiles/Makefile2 ROSBUILD_gensrv_lisp +.PHONY : ROSBUILD_gensrv_lisp + +# fast build rule for target. +ROSBUILD_gensrv_lisp/fast: + $(MAKE) -f CMakeFiles/ROSBUILD_gensrv_lisp.dir/build.make CMakeFiles/ROSBUILD_gensrv_lisp.dir/build +.PHONY : ROSBUILD_gensrv_lisp/fast + +#============================================================================= +# Target rules for targets named bh262_node + +# Build rule for target. +bh262_node: cmake_check_build_system + $(MAKE) -f CMakeFiles/Makefile2 bh262_node +.PHONY : bh262_node + +# fast build rule for target. +bh262_node/fast: + $(MAKE) -f CMakeFiles/bh262_node.dir/build.make CMakeFiles/bh262_node.dir/build +.PHONY : bh262_node/fast + +#============================================================================= +# Target rules for targets named clean-test-results + +# Build rule for target. +clean-test-results: cmake_check_build_system + $(MAKE) -f CMakeFiles/Makefile2 clean-test-results +.PHONY : clean-test-results + +# fast build rule for target. +clean-test-results/fast: + $(MAKE) -f CMakeFiles/clean-test-results.dir/build.make CMakeFiles/clean-test-results.dir/build +.PHONY : clean-test-results/fast + +#============================================================================= +# Target rules for targets named rosbuild_precompile + +# Build rule for target. +rosbuild_precompile: cmake_check_build_system + $(MAKE) -f CMakeFiles/Makefile2 rosbuild_precompile +.PHONY : rosbuild_precompile + +# fast build rule for target. +rosbuild_precompile/fast: + $(MAKE) -f CMakeFiles/rosbuild_precompile.dir/build.make CMakeFiles/rosbuild_precompile.dir/build +.PHONY : rosbuild_precompile/fast + +#============================================================================= +# Target rules for targets named rosbuild_premsgsrvgen + +# Build rule for target. +rosbuild_premsgsrvgen: cmake_check_build_system + $(MAKE) -f CMakeFiles/Makefile2 rosbuild_premsgsrvgen +.PHONY : rosbuild_premsgsrvgen + +# fast build rule for target. +rosbuild_premsgsrvgen/fast: + $(MAKE) -f CMakeFiles/rosbuild_premsgsrvgen.dir/build.make CMakeFiles/rosbuild_premsgsrvgen.dir/build +.PHONY : rosbuild_premsgsrvgen/fast + +#============================================================================= +# Target rules for targets named rospack_genmsg + +# Build rule for target. +rospack_genmsg: cmake_check_build_system + $(MAKE) -f CMakeFiles/Makefile2 rospack_genmsg +.PHONY : rospack_genmsg + +# fast build rule for target. +rospack_genmsg/fast: + $(MAKE) -f CMakeFiles/rospack_genmsg.dir/build.make CMakeFiles/rospack_genmsg.dir/build +.PHONY : rospack_genmsg/fast + +#============================================================================= +# Target rules for targets named rospack_genmsg_libexe + +# Build rule for target. +rospack_genmsg_libexe: cmake_check_build_system + $(MAKE) -f CMakeFiles/Makefile2 rospack_genmsg_libexe +.PHONY : rospack_genmsg_libexe + +# fast build rule for target. +rospack_genmsg_libexe/fast: + $(MAKE) -f CMakeFiles/rospack_genmsg_libexe.dir/build.make CMakeFiles/rospack_genmsg_libexe.dir/build +.PHONY : rospack_genmsg_libexe/fast + +#============================================================================= +# Target rules for targets named rospack_gensrv + +# Build rule for target. +rospack_gensrv: cmake_check_build_system + $(MAKE) -f CMakeFiles/Makefile2 rospack_gensrv +.PHONY : rospack_gensrv + +# fast build rule for target. +rospack_gensrv/fast: + $(MAKE) -f CMakeFiles/rospack_gensrv.dir/build.make CMakeFiles/rospack_gensrv.dir/build +.PHONY : rospack_gensrv/fast + +#============================================================================= +# Target rules for targets named test + +# Build rule for target. +test: cmake_check_build_system + $(MAKE) -f CMakeFiles/Makefile2 test +.PHONY : test + +# fast build rule for target. +test/fast: + $(MAKE) -f CMakeFiles/test.dir/build.make CMakeFiles/test.dir/build +.PHONY : test/fast + +#============================================================================= +# Target rules for targets named test-future + +# Build rule for target. +test-future: cmake_check_build_system + $(MAKE) -f CMakeFiles/Makefile2 test-future +.PHONY : test-future + +# fast build rule for target. +test-future/fast: + $(MAKE) -f CMakeFiles/test-future.dir/build.make CMakeFiles/test-future.dir/build +.PHONY : test-future/fast + +#============================================================================= +# Target rules for targets named test-results + +# Build rule for target. +test-results: cmake_check_build_system + $(MAKE) -f CMakeFiles/Makefile2 test-results +.PHONY : test-results + +# fast build rule for target. +test-results/fast: + $(MAKE) -f CMakeFiles/test-results.dir/build.make CMakeFiles/test-results.dir/build +.PHONY : test-results/fast + +#============================================================================= +# Target rules for targets named test-results-run + +# Build rule for target. +test-results-run: cmake_check_build_system + $(MAKE) -f CMakeFiles/Makefile2 test-results-run +.PHONY : test-results-run + +# fast build rule for target. +test-results-run/fast: + $(MAKE) -f CMakeFiles/test-results-run.dir/build.make CMakeFiles/test-results-run.dir/build +.PHONY : test-results-run/fast + +#============================================================================= +# Target rules for targets named tests + +# Build rule for target. +tests: cmake_check_build_system + $(MAKE) -f CMakeFiles/Makefile2 tests +.PHONY : tests + +# fast build rule for target. +tests/fast: + $(MAKE) -f CMakeFiles/tests.dir/build.make CMakeFiles/tests.dir/build +.PHONY : tests/fast + +# target to build an object file +src/bh262_node.o: + $(MAKE) -f CMakeFiles/bh262_node.dir/build.make CMakeFiles/bh262_node.dir/src/bh262_node.o +.PHONY : src/bh262_node.o + +# target to preprocess a source file +src/bh262_node.i: + $(MAKE) -f CMakeFiles/bh262_node.dir/build.make CMakeFiles/bh262_node.dir/src/bh262_node.i +.PHONY : src/bh262_node.i + +# target to generate assembly for a file +src/bh262_node.s: + $(MAKE) -f CMakeFiles/bh262_node.dir/build.make CMakeFiles/bh262_node.dir/src/bh262_node.s +.PHONY : src/bh262_node.s + +# Help Target +help: + @echo "The following are some of the valid targets for this Makefile:" + @echo "... all (the default if no target is provided)" + @echo "... clean" + @echo "... depend" + @echo "... ROSBUILD_genmsg_cpp" + @echo "... ROSBUILD_genmsg_lisp" + @echo "... ROSBUILD_gensrv_cpp" + @echo "... ROSBUILD_gensrv_lisp" + @echo "... bh262_node" + @echo "... clean-test-results" + @echo "... edit_cache" + @echo "... rebuild_cache" + @echo "... rosbuild_precompile" + @echo "... rosbuild_premsgsrvgen" + @echo "... rospack_genmsg" + @echo "... rospack_genmsg_libexe" + @echo "... rospack_gensrv" + @echo "... test" + @echo "... test-future" + @echo "... test-results" + @echo "... test-results-run" + @echo "... tests" + @echo "... src/bh262_node.o" + @echo "... src/bh262_node.i" + @echo "... src/bh262_node.s" +.PHONY : help + + + +#============================================================================= +# Special targets to cleanup operation of make. + +# Special rule to run CMake to check the build system integrity. +# No rule that depends on this can have commands that come from listfiles +# because they might be regenerated. +cmake_check_build_system: + $(CMAKE_COMMAND) -H$(CMAKE_SOURCE_DIR) -B$(CMAKE_BINARY_DIR) --check-build-system CMakeFiles/Makefile.cmake 0 +.PHONY : cmake_check_build_system + diff --git a/bh280_standalone/bh262_node/launch/bh262.launch b/bh280_standalone/bh262_node/launch/bh262.launch new file mode 100644 index 0000000..f9da925 --- /dev/null +++ b/bh280_standalone/bh262_node/launch/bh262.launch @@ -0,0 +1,4 @@ + + + + diff --git a/bh280_standalone/bh262_node/manifest.xml b/bh280_standalone/bh262_node/manifest.xml new file mode 100644 index 0000000..ffb690e --- /dev/null +++ b/bh280_standalone/bh262_node/manifest.xml @@ -0,0 +1,17 @@ + + + + This package allows for control of the BarrettHand 262 via serial communcation. + + + Kyle Maroney, Barrett Technology Inc. + BSD + + http://ros.org/wiki/bh262_node + + + + + + + diff --git a/bh280_standalone/bh262_node/src/bh262_node.cpp b/bh280_standalone/bh262_node/src/bh262_node.cpp new file mode 100644 index 0000000..a2feb7f --- /dev/null +++ b/bh280_standalone/bh262_node/src/bh262_node.cpp @@ -0,0 +1,293 @@ +/* + File: bh262_node.cpp + Date: 7 June, 2012 + Author: Kyle Maroney + */ + +#include +#include "ros/ros.h" + +//BHand Services / Joint Messages +#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" + +//Boost libraries +#include +#include +#include + +const std::string bhand_port = "/dev/ttyS0"; //Serial port location - can be found with command line $ dmesg | grep tty +const double pub_freq = 100; //Hz + +//BH262 Class +class BH262Node +{ +public: + ros::NodeHandle np_, nb_; + ros::Rate pub_rate; // Default 100Hz + std::string bhand_command; + + //Services to be made available + ros::ServiceServer hand_initialize_srv, 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; + + // Our serial port variables + boost::asio::serial_port_base::baud_rate baud; + boost::asio::serial_port_base::character_size char_size; + boost::asio::serial_port_base::flow_control flow; + boost::asio::serial_port_base::parity parity; + boost::asio::serial_port_base::stop_bits stopbits; + boost::asio::io_service io; + boost::asio::serial_port port; + + BH262Node() : + np_("~"), nb_("bhand"), pub_rate(pub_freq), baud(9600), char_size(8), flow( + boost::asio::serial_port_base::flow_control::none), parity(boost::asio::serial_port_base::parity::none), stopbits( + boost::asio::serial_port_base::stop_bits::one), port(io, bhand_port) + { + } + void + init(); + + void write(std::string buf) + { + //write command out to serial port + boost::asio::write(port, boost::asio::buffer(buf)); + boost::asio::write(port, boost::asio::buffer("\015", sizeof("\015") - 1)); + } + + ~BH262Node() + { + } + + bool + handInitialize(std_srvs::Empty::Request &req, std_srvs::Empty::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 + publish(); +}; + +void BH262Node::init() +{ + ROS_INFO("Openning BHand on serial port %s", bhand_port.c_str()); + + // Set the settings + port.set_option(baud); + port.set_option(char_size); + port.set_option(flow); + port.set_option(parity); + port.set_option(stopbits); + + //Advertise the following services only if there is a BarrettHand present + hand_initialize_srv = nb_.advertiseService("initialize", &BH262Node::handInitialize, this); // bhand/initialize + hand_open_grsp_srv = nb_.advertiseService("open_grasp", &BH262Node::handOpenGrasp, this); // bhand/open_grasp + hand_close_grsp_srv = nb_.advertiseService("close_grasp", &BH262Node::handCloseGrasp, this); // bhand/close_grasp + hand_open_sprd_srv = nb_.advertiseService("open_spread", &BH262Node::handOpenSpread, this); // bhand/open_spread + hand_close_sprd_srv = nb_.advertiseService("close_spread", &BH262Node::handCloseSpread, this); // bhand/close_spread + hand_fngr_pos_srv = nb_.advertiseService("finger_pos", &BH262Node::handFingerPos, this); // bhand/finger_pos + hand_grsp_pos_srv = nb_.advertiseService("grasp_pos", &BH262Node::handGraspPos, this); // bhand/grasp_pos + hand_sprd_pos_srv = nb_.advertiseService("spread_pos", &BH262Node::handSpreadPos, this); // bhand/spread_pos + hand_fngr_vel_srv = nb_.advertiseService("finger_vel", &BH262Node::handFingerVel, this); // bhand/finger_vel + hand_grsp_vel_srv = nb_.advertiseService("grasp_vel", &BH262Node::handGraspVel, this); // bhand/grasp_vel + hand_sprd_vel_srv = nb_.advertiseService("spread_vel", &BH262Node::handSpreadVel, this); // bhand/spread_vel + + ros::AsyncSpinner spinner(2); + spinner.start(); + +} + +bool BH262Node::handInitialize(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res) +{ + ROS_INFO("BHand Initialize Request"); + + // Write the command out to serial. + write("HI"); + return true; +} + +bool BH262Node::handOpenGrasp(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res) +{ + ROS_INFO("BHand Open Request"); + write("GO"); + return true; +} + +bool BH262Node::handCloseGrasp(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res) +{ + ROS_INFO("BHand Close Request"); + write("GC"); + return true; +} + +bool BH262Node::handOpenSpread(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res) +{ + ROS_INFO("Spread Open Request"); + write("SO"); + return true; +} + +bool BH262Node::handCloseSpread(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res) +{ + ROS_INFO("Spread Close Request"); + write("SC"); + return true; +} + +bool BH262Node::handFingerPos(wam_srvs::BHandFingerPos::Request &req, wam_srvs::BHandFingerPos::Response &res) +{ + ROS_INFO("Finger Position Request"); + bhand_command = "1M " + boost::lexical_cast(ceil(req.radians[0] * 5729.578)); + write(bhand_command); + bhand_command = "2M " + boost::lexical_cast(ceil(req.radians[1] * 5729.578)); + write(bhand_command); + bhand_command = "3M " + boost::lexical_cast(ceil(req.radians[2] * 5729.578)); + write(bhand_command); + return true; +} + +bool BH262Node::handGraspPos(wam_srvs::BHandGraspPos::Request &req, wam_srvs::BHandGraspPos::Response &res) +{ + ROS_INFO("Grasp Position Request"); + bhand_command = "GM " + boost::lexical_cast(ceil(req.radians * 5729.578)); + write(bhand_command); + return true; +} + +bool BH262Node::handSpreadPos(wam_srvs::BHandSpreadPos::Request &req, wam_srvs::BHandSpreadPos::Response &res) +{ + ROS_INFO("Spread Position Request"); + bhand_command = "SM " + boost::lexical_cast(ceil(req.radians * 1002.68)); + write(bhand_command); + return true; +} + +bool BH262Node::handFingerVel(wam_srvs::BHandFingerVel::Request &req, wam_srvs::BHandFingerVel::Response &res) +{ + ROS_INFO("Finger Velocities Request"); + + if (req.velocity[0] > 0 || req.velocity[1] > 0 || req.velocity[2] > 0) + { + bhand_command = "1FSET MCV " + boost::lexical_cast(ceil(abs(req.velocity[0]))); + write(bhand_command); + bhand_command = "2FSET MCV " + boost::lexical_cast(ceil(abs(req.velocity[1]))); + write(bhand_command); + bhand_command = "3FSET MCV " + boost::lexical_cast(ceil(abs(req.velocity[2]))); + write(bhand_command); + bhand_command = "GC"; + } + else + { + bhand_command = "1FSET MOV " + boost::lexical_cast(ceil(abs(req.velocity[0]))); + write(bhand_command); + bhand_command = "2FSET MOV " + boost::lexical_cast(ceil(abs(req.velocity[1]))); + write(bhand_command); + bhand_command = "3FSET MOV " + boost::lexical_cast(ceil(abs(req.velocity[2]))); + write(bhand_command); + bhand_command = "GO"; + } + write(bhand_command); + + bhand_command = "GFSET MCV 100 "; + write(bhand_command); + bhand_command = "GFSET MOV 100 "; + write(bhand_command); + return true; +} + +bool BH262Node::handGraspVel(wam_srvs::BHandGraspVel::Request &req, wam_srvs::BHandGraspVel::Response &res) +{ + if (req.velocity > 0.0) + { + bhand_command = "GFSET MCV " + boost::lexical_cast(ceil(abs(req.velocity))); + write(bhand_command); + bhand_command = "GC"; + } + else + { + bhand_command = "GFSET MOV " + boost::lexical_cast(ceil(abs(req.velocity))); + write(bhand_command); + bhand_command = "GO"; + } + write(bhand_command); + bhand_command = "GFSET MCV 100 "; + write(bhand_command); + bhand_command = "GFSET MOV 100 "; + write(bhand_command); + return true; +} + +bool BH262Node::handSpreadVel(wam_srvs::BHandSpreadVel::Request &req, wam_srvs::BHandSpreadVel::Response &res) +{ + if (req.velocity > 0.0) + { + bhand_command = "SFSET MCV " + boost::lexical_cast(ceil(abs(req.velocity))); + write(bhand_command); + bhand_command = "SC"; + } + else + { + bhand_command = "SFSET MOV " + boost::lexical_cast(ceil(abs(req.velocity))); + write(bhand_command); + bhand_command = "SO"; + } + write(bhand_command); + bhand_command = "GFSET MCV 100 "; + write(bhand_command); + bhand_command = "GFSET MOV 100 "; + write(bhand_command); + return true; +} + +void BH262Node::publish() +{ + //write("SFGET P"); + //write("GFGET P"); + //Working on reading these positions and converting them to joint_state messages +} + +int main(int argc, char **argv) +{ + ros::init(argc, argv, "bh262_node"); + + BH262Node bhand; + bhand.init(); + + ros::Rate pub_rate(10); + + while (ros::ok()) + { + ros::spinOnce(); + bhand.publish(); + pub_rate.sleep(); + } + + bhand.port.close(); + return 0; +} diff --git a/bh280_standalone/launch/bh280.launch b/bh280_standalone/launch/bh280.launch new file mode 100644 index 0000000..100ea36 --- /dev/null +++ b/bh280_standalone/launch/bh280.launch @@ -0,0 +1,4 @@ + + + + diff --git a/bh280_standalone/manifest.xml b/bh280_standalone/manifest.xml new file mode 100644 index 0000000..7590e5c --- /dev/null +++ b/bh280_standalone/manifest.xml @@ -0,0 +1,17 @@ + + + + Barrett Technology BH8-280 Standalone node to be run via CAN. + + + Barrett Technology Inc., Kyle Maroney + GPL + http://ros.org/wiki/bh280_standalone + + + + + + + + diff --git a/bh280_standalone/src/bh280_node.cpp b/bh280_standalone/src/bh280_node.cpp new file mode 100644 index 0000000..6630c32 --- /dev/null +++ b/bh280_standalone/src/bh280_node.cpp @@ -0,0 +1,213 @@ +#include "ros/ros.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 +#include +#include + +static const int PUBLISH_FREQ = 50; // Default Control Loop / Publishing Frequency + +using namespace barrett; + +//BHandNode Class +class BHandNode +{ +public: + ros::NodeHandle n_; // BarrettHand specific nodehandle + Hand* hand; + + //Published Topics + sensor_msgs::JointState wam_joint_state, bhand_joint_state; + + //Publishers + ros::Publisher wam_joint_state_pub, bhand_joint_state_pub, wam_pose_pub; + + //Services + ros::ServiceServer hand_init_srv, hand_open_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; + + BHandNode(Hand* hand_) : + n_("bhand"), hand(hand_) + { + } + void + init(); + + ~BHandNode() + { + } + + bool + initialize(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res); + bool + openGrasp(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res); + bool + closeGrasp(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res); + bool + openSpread(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res); + bool + closeSpread(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res); + bool + fingerPos(wam_srvs::BHandFingerPos::Request &req, wam_srvs::BHandFingerPos::Response &res); + bool + graspPos(wam_srvs::BHandGraspPos::Request &req, wam_srvs::BHandGraspPos::Response &res); + bool + spreadPos(wam_srvs::BHandSpreadPos::Request &req, wam_srvs::BHandSpreadPos::Response &res); + bool + fingerVel(wam_srvs::BHandFingerVel::Request &req, wam_srvs::BHandFingerVel::Response &res); + bool + graspVel(wam_srvs::BHandGraspVel::Request &req, wam_srvs::BHandGraspVel::Response &res); + bool + spreadVel(wam_srvs::BHandSpreadVel::Request &req, wam_srvs::BHandSpreadVel::Response &res); + void + publish(void); +}; + +void BHandNode::init() +{ + bhand_joint_state_pub = n_.advertise < sensor_msgs::JointState > ("joint_states", 1); // bhand/joint_states + + hand_init_srv = n_.advertiseService("initialize", &BHandNode::initialize, this); // bhand/initialize + hand_open_grsp_srv = n_.advertiseService("open_grasp", &BHandNode::openGrasp, this); // bhand/open_grasp + hand_close_grsp_srv = n_.advertiseService("close_grasp", &BHandNode::closeGrasp, this); // bhand/close_grasp + hand_open_sprd_srv = n_.advertiseService("open_spread", &BHandNode::openSpread, this); // bhand/open_spread + hand_close_sprd_srv = n_.advertiseService("close_spread", &BHandNode::closeSpread, this); // bhand/close_spread + hand_fngr_pos_srv = n_.advertiseService("finger_pos", &BHandNode::fingerPos, this); // bhand/finger_pos + hand_grsp_pos_srv = n_.advertiseService("grasp_pos", &BHandNode::graspPos, this); // bhand/grasp_pos + hand_sprd_pos_srv = n_.advertiseService("spread_pos", &BHandNode::spreadPos, this); // bhand/spread_pos + hand_fngr_vel_srv = n_.advertiseService("finger_vel", &BHandNode::fingerVel, this); // bhand/finger_vel + hand_grsp_vel_srv = n_.advertiseService("grasp_vel", &BHandNode::graspVel, this); // bhand/grasp_vel + hand_sprd_vel_srv = n_.advertiseService("spread_vel", &BHandNode::spreadVel, 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); +} + +bool BHandNode::initialize(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res) +{ + hand->initialize(); + hand->update(); + return true; +} +bool BHandNode::openGrasp(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res) +{ + ROS_INFO("Opening the BarrettHand Grasp"); + hand->open(Hand::GRASP, false); + return true; +} +bool BHandNode::closeGrasp(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res) +{ + ROS_INFO("Closing the BarrettHand Grasp"); + hand->close(Hand::GRASP, false); + return true; +} +bool BHandNode::openSpread(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res) +{ + ROS_INFO("Opening the BarrettHand Spread"); + hand->open(Hand::SPREAD, false); + return true; +} +bool BHandNode::closeSpread(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res) +{ + ROS_INFO("Closing the BarrettHand Spread"); + hand->close(Hand::SPREAD, false); + return true; +} +bool BHandNode::fingerPos(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; +} +bool BHandNode::graspPos(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; +} +bool BHandNode::spreadPos(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; +} +bool BHandNode::fingerVel(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; +} +bool BHandNode::graspVel(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; +} +bool BHandNode::spreadVel(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; +} +void BHandNode::publish(void) +{ + 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 / PUBLISH_FREQ); // Sleep according to the specified publishing frequency +} + +int main(int argc, char** argv) +{ + ros::init(argc, argv, "bhand_node"); + + ProductManager pm; + if (!pm.foundHand()) + { + printf("ERROR: No Hand found on bus!\n"); + return 1; + } + + Hand* hand = pm.getHand(); + + BHandNode bhand_node(hand); + + bhand_node.init(); + + ros::Rate pub_rate(PUBLISH_FREQ); + + while (bhand_node.n_.ok()) + { + ros::spinOnce(); + bhand_node.publish(); + pub_rate.sleep(); + } + + hand->idle(); + + return 0; +}