--- /dev/null
+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)
+
--- /dev/null
+# 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
+
--- /dev/null
+<launch>
+ <!-- BHand Node -->
+ <node name="bh262_nod" type="bh262_node" pkg="bh262_node" output="screen" />
+</launch>
--- /dev/null
+<package>
+ <description brief="bh262_node">
+
+ This package allows for control of the BarrettHand 262 via serial communcation.
+
+ </description>
+ <author>Kyle Maroney, Barrett Technology Inc.</author>
+ <license>BSD</license>
+ <review status="unreviewed" notes=""/>
+ <url>http://ros.org/wiki/bh262_node</url>
+ <depend package="roscpp"/>
+ <depend package="wam_srvs"/>
+ <depend package="std_srvs"/>
+ <depend package="sensor_msgs"/>
+</package>
+
+
--- /dev/null
+/*
+ File: bh262_node.cpp
+ Date: 7 June, 2012
+ Author: Kyle Maroney
+ */
+
+#include <string>
+#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 <boost/thread.hpp>
+#include <boost/asio.hpp>
+#include <boost/lexical_cast.hpp>
+
+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<std::string>(ceil(req.radians[0] * 5729.578));
+ write(bhand_command);
+ bhand_command = "2M " + boost::lexical_cast<std::string>(ceil(req.radians[1] * 5729.578));
+ write(bhand_command);
+ bhand_command = "3M " + boost::lexical_cast<std::string>(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<std::string>(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<std::string>(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<std::string>(ceil(abs(req.velocity[0])));
+ write(bhand_command);
+ bhand_command = "2FSET MCV " + boost::lexical_cast<std::string>(ceil(abs(req.velocity[1])));
+ write(bhand_command);
+ bhand_command = "3FSET MCV " + boost::lexical_cast<std::string>(ceil(abs(req.velocity[2])));
+ write(bhand_command);
+ bhand_command = "GC";
+ }
+ else
+ {
+ bhand_command = "1FSET MOV " + boost::lexical_cast<std::string>(ceil(abs(req.velocity[0])));
+ write(bhand_command);
+ bhand_command = "2FSET MOV " + boost::lexical_cast<std::string>(ceil(abs(req.velocity[1])));
+ write(bhand_command);
+ bhand_command = "3FSET MOV " + boost::lexical_cast<std::string>(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<std::string>(ceil(abs(req.velocity)));
+ write(bhand_command);
+ bhand_command = "GC";
+ }
+ else
+ {
+ bhand_command = "GFSET MOV " + boost::lexical_cast<std::string>(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<std::string>(ceil(abs(req.velocity)));
+ write(bhand_command);
+ bhand_command = "SC";
+ }
+ else
+ {
+ bhand_command = "SFSET MOV " + boost::lexical_cast<std::string>(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;
+}
--- /dev/null
+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})
+
--- /dev/null
+# 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
+
--- /dev/null
+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)
+
--- /dev/null
+# 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
+
--- /dev/null
+<launch>
+ <!-- BHand Node -->
+ <node name="bh262_nod" type="bh262_node" pkg="bh262_node" output="screen" />
+</launch>
--- /dev/null
+<package>
+ <description brief="bh262_node">
+
+ This package allows for control of the BarrettHand 262 via serial communcation.
+
+ </description>
+ <author>Kyle Maroney, Barrett Technology Inc.</author>
+ <license>BSD</license>
+ <review status="unreviewed" notes=""/>
+ <url>http://ros.org/wiki/bh262_node</url>
+ <depend package="roscpp"/>
+ <depend package="wam_srvs"/>
+ <depend package="std_srvs"/>
+ <depend package="sensor_msgs"/>
+</package>
+
+
--- /dev/null
+/*
+ File: bh262_node.cpp
+ Date: 7 June, 2012
+ Author: Kyle Maroney
+ */
+
+#include <string>
+#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 <boost/thread.hpp>
+#include <boost/asio.hpp>
+#include <boost/lexical_cast.hpp>
+
+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<std::string>(ceil(req.radians[0] * 5729.578));
+ write(bhand_command);
+ bhand_command = "2M " + boost::lexical_cast<std::string>(ceil(req.radians[1] * 5729.578));
+ write(bhand_command);
+ bhand_command = "3M " + boost::lexical_cast<std::string>(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<std::string>(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<std::string>(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<std::string>(ceil(abs(req.velocity[0])));
+ write(bhand_command);
+ bhand_command = "2FSET MCV " + boost::lexical_cast<std::string>(ceil(abs(req.velocity[1])));
+ write(bhand_command);
+ bhand_command = "3FSET MCV " + boost::lexical_cast<std::string>(ceil(abs(req.velocity[2])));
+ write(bhand_command);
+ bhand_command = "GC";
+ }
+ else
+ {
+ bhand_command = "1FSET MOV " + boost::lexical_cast<std::string>(ceil(abs(req.velocity[0])));
+ write(bhand_command);
+ bhand_command = "2FSET MOV " + boost::lexical_cast<std::string>(ceil(abs(req.velocity[1])));
+ write(bhand_command);
+ bhand_command = "3FSET MOV " + boost::lexical_cast<std::string>(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<std::string>(ceil(abs(req.velocity)));
+ write(bhand_command);
+ bhand_command = "GC";
+ }
+ else
+ {
+ bhand_command = "GFSET MOV " + boost::lexical_cast<std::string>(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<std::string>(ceil(abs(req.velocity)));
+ write(bhand_command);
+ bhand_command = "SC";
+ }
+ else
+ {
+ bhand_command = "SFSET MOV " + boost::lexical_cast<std::string>(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;
+}
--- /dev/null
+<launch>
+ <!-- BHand Node -->
+ <node name="bh280_node" type="bh280_node" pkg="bh280_standalone" output="screen" />
+</launch>
--- /dev/null
+<package>
+ <description brief="bh280_standalone">
+
+ Barrett Technology BH8-280 Standalone node to be run via CAN.
+
+ </description>
+ <author>Barrett Technology Inc., Kyle Maroney</author>
+ <license>GPL</license>
+ <url>http://ros.org/wiki/bh280_standalone</url>
+ <depend package="std_msgs"/>
+ <depend package="std_srvs"/>
+ <depend package="sensor_msgs"/>
+ <depend package="roscpp"/>
+ <depend package="wam_srvs"/>
+</package>
+
+
--- /dev/null
+#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 <barrett/os.h>
+#include <barrett/math.h>
+#include <barrett/products/product_manager.h>
+
+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;
+}
projects / barrett-ros-pkg.git / commitdiff