From 045f5406fdf74027cc6e900e4b39fcbca63dfc4f Mon Sep 17 00:00:00 2001 From: Hariharasudan Date: Fri, 17 Oct 2014 11:41:31 -0400 Subject: [PATCH 1/7] Modularized the code to have separate threads for hand and the arm --- CMakeLists.txt | 13 ++--- src/barrett_arm.cpp => include/barrett_arm.h | 58 +++++++++----------- src/hand.cpp | 39 ------------- 3 files changed, 33 insertions(+), 77 deletions(-) rename src/barrett_arm.cpp => include/barrett_arm.h (76%) delete mode 100644 src/hand.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 47c8ee7..c5bf5fd 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -12,23 +12,22 @@ find_package(Barrett REQUIRED) find_package(Eigen REQUIRED) catkin_package( + INCLUDE_DIRS include CATKIN_DEPENDS message_runtime wam_msgs + LIBRARIES ${PROJECT_NAME} ) -include_directories( +include_directories(include ${catkin_INCLUDE_DIRS} ) link_directories(${BARRETT_LIB_DIRS}) include_directories(${BARRETT_INCLUDE_DIRS}) add_definitions(${BARRETT_DEFINITIONS}) -add_executable(barrett_arm src/barrett_arm.cpp) -add_dependencies(barrett_arm wam_msgs_gencpp) -target_link_libraries(barrett_arm ${BARRETT_LIBRARIES} ${catkin_LIBRARIES}) +add_executable(${PROJECT_NAME} src/libbarrett_ros.cpp) +target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} ${BARRETT_LIBRARIES}) -add_executable(hand src/hand.cpp) -add_dependencies(hand wam_msgs_gencpp) -target_link_libraries(hand ${BARRETT_LIBRARIES} ${catkin_LIBRARIES}) +add_dependencies(${PROJECT_NAME} ${catkin_EXPORTED_TARGETS} wam_msgs_gencpp) diff --git a/src/barrett_arm.cpp b/include/barrett_arm.h similarity index 76% rename from src/barrett_arm.cpp rename to include/barrett_arm.h index ce3c542..e44c74f 100644 --- a/src/barrett_arm.cpp +++ b/include/barrett_arm.h @@ -1,10 +1,13 @@ /* - * This node is a ros wrapper for the basic functionalities of a wam arm. This - * node holds the position, the orientation or both position and orientation as - * vommanded by the subscribing topic. - * It also displays the basic info of the wam such as the Joint Position, Velocity, + * barrett_arm.h + * + * Created on: Oct 16, 2014 + * Author: robot */ +#ifndef BARRETT_ARM_H_ +#define BARRETT_ARM_H_ + #include "ros/ros.h" #include #include @@ -24,7 +27,7 @@ #include using namespace barrett; -const std::string BARRETT_ARM_CONTROL_TOPIC = "barrett/wam/control"; +const std::string BARRETT_ARM_CONTROL_TOPIC = "barrett/arm/control"; const std::string BARRETT_ARM_JS_TOPIC = "barrett/arm/joint_state"; const std::string BARRETT_ARM_ES_TOPIC = "barrett/arm/endpoint_state"; @@ -48,18 +51,21 @@ class barrett_arm_interface{ sensor_msgs::JointState js; wam_msgs::EndpointState es; - systems::Wam * arm_wam; + systems::Wam* arm_wam; + ProductManager* arm_pm; ros::NodeHandle nh; ros::Subscriber barrett_arm_control_sub; ros::Publisher barrett_arm_js_pub, barrett_arm_es_pub; void barrett_arm_control_modes(const wam_msgs::RTPosMode& msg); - void publish_barrett_arm_info(systems::Wam &wam); + void publish_barrett_arm_info(); public: - barrett_arm_interface(ProductManager &pm, systems::Wam &wam); + barrett_arm_interface(ProductManager &pm, systems::Wam &wam): arm_wam(&wam), arm_pm(&pm){}; + void start(); }; + template void barrett_arm_interface::barrett_arm_control_modes(const wam_msgs::RTPosMode& msg){ switch (msg.mode) { @@ -98,15 +104,15 @@ void barrett_arm_interface::barrett_arm_control_modes(const wam_msgs::RTPos * Publish the Joint and the endpoint states of the wam */ template -void barrett_arm_interface::publish_barrett_arm_info(systems::Wam &wam){ +void barrett_arm_interface::publish_barrett_arm_info(){ - jp=wam.getJointPositions(); - jv=wam.getJointVelocities(); - jt=wam.getJointTorques(); + jp = arm_wam->getJointPositions(); + jv = arm_wam->getJointVelocities(); + jt = arm_wam->getJointTorques(); - cp=wam.getToolPosition(); - cv=wam.getToolVelocity(); - qt = wam.getToolOrientation(); + cp = arm_wam->getToolPosition(); + cv = arm_wam->getToolVelocity(); + qt = arm_wam->getToolOrientation(); //Pack the Joint and Endpoint state messages with the updated values for(size_t i = 0; i < DOF; ++i){ @@ -132,7 +138,7 @@ void barrett_arm_interface::publish_barrett_arm_info(systems::Wam &wam * Initialize the subscribers and publishers. Initialize the messages with their default values */ template -barrett_arm_interface::barrett_arm_interface(ProductManager &pm, systems::Wam &wam): arm_wam(&wam){ +void barrett_arm_interface::start(){ barrett_arm_control_sub = nh.subscribe(BARRETT_ARM_CONTROL_TOPIC, 1, &barrett_arm_interface::barrett_arm_control_modes, this); @@ -151,27 +157,17 @@ barrett_arm_interface::barrett_arm_interface(ProductManager &pm, systems::W ros::Rate loop_rate(200); //Turn on the gravity compensation by default - wam.gravityCompensate(); + arm_wam->gravityCompensate(); while(ros::ok){ - publish_barrett_arm_info(wam); + publish_barrett_arm_info(); ros::spinOnce(); loop_rate.sleep(); } //Idle the Wam - wam.idle(); - pm.getSafetyModule()->waitForMode(SafetyModule::IDLE); -} - -template -int wam_main(int argc, char** argv, ProductManager& pm, systems::Wam& wam) { - - barrett_arm_interface w_if(pm, wam); - - return 0; + arm_wam->idle(); + arm_pm->getSafetyModule()->waitForMode(SafetyModule::IDLE); } - - - +#endif /* BARRETT_ARM_H_ */ diff --git a/src/hand.cpp b/src/hand.cpp deleted file mode 100644 index b539812..0000000 --- a/src/hand.cpp +++ /dev/null @@ -1,39 +0,0 @@ -/* - * hand.cpp - * - * Created on: Oct 15, 2014 - * Author: robot - */ - - -#include "ros/ros.h" -// The file below provides access to the barrett::units namespace. -#include -#include -#include -#include -#include -#include - -#include - -using namespace barrett; -using detail::waitForEnter; - -template -int wam_main(int argc, char** argv, ProductManager& pm, systems::Wam& wam) { - - // Attempt to connect to the hand (BH-280 or newer) - Hand* hand = NULL; - if (pm.foundHand()) { - hand = pm.getHand(); - std::cout << ">>> Press [Enter] to initialize Hand. (Make sure it has room!)"; - waitForEnter(); - hand->initialize(); - } else { - ROS_ERROR("No hand detected! Quitting..."); - // Wait for the user to press Shift+Idle - pm.getSafetyModule()->waitForMode(SafetyModule::IDLE); - return 0; - } -} From 056d6ce643ee3287937047b914bb6a062bd17e19 Mon Sep 17 00:00:00 2001 From: Hariharasudan Date: Fri, 17 Oct 2014 11:49:35 -0400 Subject: [PATCH 2/7] Included the src --- src/libbarrett_ros.cpp | 27 +++++++++++++++++++++++++++ 1 file changed, 27 insertions(+) create mode 100644 src/libbarrett_ros.cpp diff --git a/src/libbarrett_ros.cpp b/src/libbarrett_ros.cpp new file mode 100644 index 0000000..106fa1b --- /dev/null +++ b/src/libbarrett_ros.cpp @@ -0,0 +1,27 @@ +/* + * libbarrett_ros.cpp + * + * Created on: Oct 16, 2014 + * Author: Hariharasudan Malaichamee + */ +#include "ros/ros.h" +#include +#include +#include + +#include "../include/barrett_arm.h" + +using namespace barrett; +template +int wam_main(int argc, char** argv, ProductManager& pm, systems::Wam& wam) { + + ros::init(argc, argv, "libbarrett_interface"); + + barrett_arm_interface barrett_arm( pm, wam); + + //Create thread for the individual products + boost::thread arm_thread(&barrett_arm_interface::start, &barrett_arm); + + arm_thread.join(); + return 0; +} From e9ae083c31b8d58950ce5ace0c63f6584234b64e Mon Sep 17 00:00:00 2001 From: Hariharasudan Date: Fri, 17 Oct 2014 14:08:54 -0400 Subject: [PATCH 3/7] Modified the semantics --- include/barrett_arm.h | 36 ++++++++++++++++++------------------ 1 file changed, 18 insertions(+), 18 deletions(-) diff --git a/include/barrett_arm.h b/include/barrett_arm.h index e44c74f..dd817e3 100644 --- a/include/barrett_arm.h +++ b/include/barrett_arm.h @@ -27,14 +27,14 @@ #include using namespace barrett; -const std::string BARRETT_ARM_CONTROL_TOPIC = "barrett/arm/control"; -const std::string BARRETT_ARM_JS_TOPIC = "barrett/arm/joint_state"; -const std::string BARRETT_ARM_ES_TOPIC = "barrett/arm/endpoint_state"; +const std::string ARM_CONTROL_TOPIC = "arm/control"; +const std::string ARM_JS_TOPIC = "arm/joint_states"; +const std::string ARM_ES_TOPIC = "arm/endpoint_state"; const std::string jnt_names[] = {"j1", "j2", "j3", "j4", "j5", "j6", "j7"}; template -class barrett_arm_interface{ +class BarrettArmInterface{ BARRETT_UNITS_TEMPLATE_TYPEDEFS(DOF); //WAM information for retrieval @@ -54,20 +54,20 @@ class barrett_arm_interface{ systems::Wam* arm_wam; ProductManager* arm_pm; ros::NodeHandle nh; - ros::Subscriber barrett_arm_control_sub; - ros::Publisher barrett_arm_js_pub, barrett_arm_es_pub; + ros::Subscriber arm_control_sub; + ros::Publisher arm_js_pub, arm_es_pub; - void barrett_arm_control_modes(const wam_msgs::RTPosMode& msg); - void publish_barrett_arm_info(); + void armControlModes(const wam_msgs::RTPosMode&); + void armPublishInfo(); public: - barrett_arm_interface(ProductManager &pm, systems::Wam &wam): arm_wam(&wam), arm_pm(&pm){}; + BarrettArmInterface(ProductManager &pm, systems::Wam &wam): arm_wam(&wam), arm_pm(&pm){}; void start(); }; template -void barrett_arm_interface::barrett_arm_control_modes(const wam_msgs::RTPosMode& msg){ +void BarrettArmInterface::armControlModes(const wam_msgs::RTPosMode& msg){ switch (msg.mode) { case wam_msgs::RTPosMode::JOINT_POSITION_CONTROL: printf("Holding joint positions.\n"); @@ -104,7 +104,7 @@ void barrett_arm_interface::barrett_arm_control_modes(const wam_msgs::RTPos * Publish the Joint and the endpoint states of the wam */ template -void barrett_arm_interface::publish_barrett_arm_info(){ +void BarrettArmInterface::armPublishInfo(){ jp = arm_wam->getJointPositions(); jv = arm_wam->getJointVelocities(); @@ -130,23 +130,23 @@ void barrett_arm_interface::publish_barrett_arm_info(){ es.orientation.z = qt.z(); es.orientation.w = qt.w(); - barrett_arm_js_pub.publish(js); - barrett_arm_es_pub.publish(es); + arm_js_pub.publish(js); + arm_es_pub.publish(es); } /* * Initialize the subscribers and publishers. Initialize the messages with their default values */ template -void barrett_arm_interface::start(){ +void BarrettArmInterface::start(){ - barrett_arm_control_sub = nh.subscribe(BARRETT_ARM_CONTROL_TOPIC, 1, &barrett_arm_interface::barrett_arm_control_modes, this); + arm_control_sub = nh.subscribe(ARM_CONTROL_TOPIC, 1, &BarrettArmInterface::armControlModes, this); //Joint Publishers - barrett_arm_js_pub = nh.advertise(BARRETT_ARM_JS_TOPIC, 1); + arm_js_pub = nh.advertise(ARM_JS_TOPIC, 1); //Cartesian Publishers - barrett_arm_es_pub = nh.advertise(BARRETT_ARM_ES_TOPIC, 1); + arm_es_pub = nh.advertise(ARM_ES_TOPIC, 1); //Initialize the Joint state publisher message with its default values for (size_t i=0; i::start(){ arm_wam->gravityCompensate(); while(ros::ok){ - publish_barrett_arm_info(); + armPublishInfo(); ros::spinOnce(); loop_rate.sleep(); } From 1aeaefe5251dfb7531382810a27d116c7b9f778f Mon Sep 17 00:00:00 2001 From: Hariharasudan Date: Fri, 17 Oct 2014 14:31:47 -0400 Subject: [PATCH 4/7] Added a separate header for barrett hand and modified the barrett arm --- include/barrett_arm.h | 4 +- include/barrett_hand.h | 269 +++++++++++++++++++++++++++++++++++++++++ 2 files changed, 272 insertions(+), 1 deletion(-) create mode 100644 include/barrett_hand.h diff --git a/include/barrett_arm.h b/include/barrett_arm.h index e44c74f..ea7dadc 100644 --- a/include/barrett_arm.h +++ b/include/barrett_arm.h @@ -27,6 +27,8 @@ #include using namespace barrett; +const int RATE = 200; //Execution rate in Hz + const std::string BARRETT_ARM_CONTROL_TOPIC = "barrett/arm/control"; const std::string BARRETT_ARM_JS_TOPIC = "barrett/arm/joint_state"; const std::string BARRETT_ARM_ES_TOPIC = "barrett/arm/endpoint_state"; @@ -154,7 +156,7 @@ void barrett_arm_interface::start(){ } //Set the loop rate at 200 Hz - ros::Rate loop_rate(200); + ros::Rate loop_rate(RATE); //Turn on the gravity compensation by default arm_wam->gravityCompensate(); diff --git a/include/barrett_hand.h b/include/barrett_hand.h new file mode 100644 index 0000000..fb4a156 --- /dev/null +++ b/include/barrett_hand.h @@ -0,0 +1,269 @@ +#include "ros/ros.h" +// The file below provides access to the barrett::units namespace. +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +#include + + +using namespace barrett; + +const int RATE = 200; //Execution rate in Hz +const char* bhand_jnts[] = {"inner_f1", "inner_f2", "inner_f3", "spread", "outer_f1", "outer_f2", "outer_f3"}; + +const std::string HAND_JS_TOPIC = "hand/joint_states"; + +const std::string HAND_OPN_GRSP_SRV = "hand/open_grasp"; +const std::string HAND_CLS_GRSP_SRV = "hand/close_grasp"; +const std::string HAND_OPN_SPRD_SRV = "hand/open_spread"; +const std::string HAND_CLS_SPRD_SRV = "hand/close_spread"; +const std::string HAND_FNGR_POS_SRV = "hand/finger_pos"; +const std::string HAND_FNGR_VEL_SRV = "hand/finger_vel"; +const std::string HAND_GRSP_POS_SRV = "hand/grasp_pos"; +const std::string HAND_GRSP_VEL_SRV = "hand/grasp_vel"; +const std::string HAND_SPRD_POS_SRV = "hand/spread_pos"; +const std::string HAND_SPRD_VEL_SRV = "hand/spread_vel"; + +template +class BarrettHandInterface{ + BARRETT_UNITS_TEMPLATE_TYPEDEFS(DOF); + + bool isInitialized; //Tracks if the hand is initialized + std_srvs::Empty::Request def_req; + std_srvs::Empty::Response def_res; + sensor_msgs::JointState js; + + systems::Wam* hand_wam; + ProductManager* hand_pm; + Hand* hand; + ros::NodeHandle nh; + ros::Publisher hand_js_pub; + 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; + + bool handInitialize(std_srvs::Empty::Request &, std_srvs::Empty::Response &); + bool handOpenGrasp(std_srvs::Empty::Request &, std_srvs::Empty::Response &); + bool handCloseGrasp(std_srvs::Empty::Request &, std_srvs::Empty::Response &); + bool handOpenSpread(std_srvs::Empty::Request &, std_srvs::Empty::Response &); + bool handCloseSpread(std_srvs::Empty::Request &, std_srvs::Empty::Response &); + bool handFingerPos(wam_msgs::BHandFingerPos::Request &, wam_msgs::BHandFingerPos::Response &); + bool handFingerVel(wam_msgs::BHandFingerVel::Request &, wam_msgs::BHandFingerVel::Response &); + bool handGraspPos(wam_msgs::BHandGraspPos::Request &, wam_msgs::BHandGraspPos::Response &); + bool handGraspVel(wam_msgs::BHandGraspVel::Request &, wam_msgs::BHandGraspVel::Response &); + bool handSpreadPos(wam_msgs::BHandSpreadPos::Request &, wam_msgs::BHandSpreadPos::Response &); + bool handSpreadVel(wam_msgs::BHandSpreadVel::Request &, wam_msgs::BHandSpreadVel::Response &); + bool publishHandInfo(); + +public: + BarrettHandInterface(ProductManager &pm, systems::Wam &wam): hand_wam(&wam), hand_pm(&pm), hand(NULL), isInitialized(false){}; + void start(); +}; + +/* Initializes the hand if it is not already initialized + * returns if the hand was initialized + */ +template +bool BarrettHandInterface::handInitialize(std_srvs::Empty::Request &req = def_req, std_srvs::Empty::Response &res = def_res){ + + if(!isInitialized){ + hand->initialize(); + ROS_INFO("The Hand is Initialized"); + isInitialized = true; + } + + return true;; +} + +//Function to open the BarrettHand Grasp +template +bool BarrettHandInterface::handOpenGrasp(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res){ + + if(!isInitialized) + handInitialize(); + + ROS_INFO("Opening the BarrettHand Grasp"); + hand->open(Hand::GRASP, false); + return true; +} + +//Function to close the BarrettHand Grasp +template +bool BarrettHandInterface::handCloseGrasp(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res){ + + if(!isInitialized) + handInitialize(); + + ROS_INFO("Closing the BarrettHand Grasp"); + hand->close(Hand::GRASP, false); + return true; + } + +//Function to open the BarrettHand Spread +template +bool BarrettHandInterface::handOpenSpread(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res){ + + if(!isInitialized) + handInitialize(); + + ROS_INFO("Opening the BarrettHand Spread"); + hand->open(Hand::SPREAD, false); + return true; + } + +//Function to close the BarrettHand Spread +template +bool BarrettHandInterface::handCloseSpread(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res){ + + if(!isInitialized) + handInitialize(); + + ROS_INFO("Closing the BarrettHand Spread"); + hand->close(Hand::SPREAD, false); + return true; + } + +//Function to control a BarrettHand Finger Position +template +bool BarrettHandInterface::handFingerPos(wam_msgs::BHandFingerPos::Request &req, wam_msgs::BHandFingerPos::Response &res){ + + if(!isInitialized) + handInitialize(); + + ROS_INFO("Moving BarrettHand to Finger Positions: %.3f, %.3f, %.3f radians", req.radians[0], req.radians[1], + req.radians[2]); + hand->trapezoidalMove(Hand::jp_type(req.radians[0], req.radians[1], req.radians[2], 0.0), Hand::GRASP, false); + return true; + } + +//Function to control a BarrettHand Finger Velocity +template +bool BarrettHandInterface::handFingerVel(wam_msgs::BHandFingerVel::Request &req, wam_msgs::BHandFingerVel::Response &res){ + + if(!isInitialized) + handInitialize(); + + ROS_INFO("Moving BarrettHand Finger Velocities: %.3f, %.3f, %.3f m/s", req.velocity[0], req.velocity[1], + req.velocity[2]); + hand->velocityMove(Hand::jv_type(req.velocity[0], req.velocity[1], req.velocity[2], 0.0), Hand::GRASP); + return true; + } + +//Function to control the BarrettHand Grasp Position +template +bool BarrettHandInterface::handGraspPos(wam_msgs::BHandGraspPos::Request &req, wam_msgs::BHandGraspPos::Response &res){ + + if(!isInitialized) + handInitialize(); + + ROS_INFO("Moving BarrettHand Grasp: %.3f radians", req.radians); + hand->trapezoidalMove(Hand::jp_type(req.radians), Hand::GRASP, false); + return true; + } + +//Function to control a BarrettHand Grasp Velocity +template +bool BarrettHandInterface::handGraspVel(wam_msgs::BHandGraspVel::Request &req, wam_msgs::BHandGraspVel::Response &res){ + + if(!isInitialized) + handInitialize(); + + ROS_INFO("Moving BarrettHand Grasp: %.3f m/s", req.velocity); + hand->velocityMove(Hand::jv_type(req.velocity), Hand::GRASP); + return true; + } + +//Function to control the BarrettHand Spread Position +template +bool BarrettHandInterface::handSpreadPos(wam_msgs::BHandSpreadPos::Request &req, wam_msgs::BHandSpreadPos::Response &res){ + + if(!isInitialized) + handInitialize(); + + ROS_INFO("Moving BarrettHand Spread: %.3f radians", req.radians); + hand->trapezoidalMove(Hand::jp_type(req.radians), Hand::SPREAD, false); + return true; + } + +//Function to control a BarrettHand Spread Velocity +template +bool BarrettHandInterface::handSpreadVel(wam_msgs::BHandSpreadVel::Request &req, wam_msgs::BHandSpreadVel::Response &res){ + + if(!isInitialized) + handInitialize(); + + ROS_INFO("Moving BarrettHand Spread: %.3f m/s", req.velocity); + usleep(5000); + hand->velocityMove(Hand::jv_type(req.velocity), Hand::SPREAD); + return true; + } + +//Function to publish the joint states of the hand at desired rate +template +bool BarrettHandInterface::publishHandInfo(){ + + //Set the loop rate at 200 Hz + ros::Rate loop_rate(RATE); + + while (ros::ok()) + { + hand->update(); // Update the hand sensors + Hand::jp_type hi = hand->getInnerLinkPosition(); // get finger positions information + Hand::jp_type ho = hand->getOuterLinkPosition(); + for (size_t i = 0; i < 4; i++) // Save finger positions + js.position[i] = hi[i]; + for (size_t j = 0; j < 3; j++) + js.position[j + 4] = ho[j]; + js.header.stamp = ros::Time::now(); // Set the timestamp + hand_js_pub.publish(js); // Publish the BarrettHand joint states + loop_rate.sleep(); + } +} +template +void BarrettHandInterface::start(){ + +//Check if the hand is present + if(hand_pm->getHand()){ + + //Initalize the publishers + hand_js_pub = nh.advertise(HAND_JS_TOPIC, 1); + + //Advertise the following services only if there is a BarrettHand present + hand_open_grsp_srv = nh.advertiseService(HAND_OPN_GRSP_SRV, &BarrettHandInterface::handOpenGrasp, this); + hand_close_grsp_srv = nh.advertiseService(HAND_CLS_GRSP_SRV, &BarrettHandInterface::handCloseGrasp, this); + hand_open_sprd_srv = nh.advertiseService(HAND_OPN_SPRD_SRV, &BarrettHandInterface::handOpenSpread, this); + hand_close_sprd_srv = nh.advertiseService(HAND_CLS_SPRD_SRV, &BarrettHandInterface::handCloseSpread, this); + hand_fngr_pos_srv = nh.advertiseService(HAND_FNGR_POS_SRV, &BarrettHandInterface::handFingerPos, this); + hand_sprd_vel_srv = nh.advertiseService(HAND_FNGR_VEL_SRV, &BarrettHandInterface::handSpreadVel, this); + hand_grsp_pos_srv = nh.advertiseService(HAND_GRSP_POS_SRV, &BarrettHandInterface::handGraspPos, this); + hand_grsp_vel_srv = nh.advertiseService(HAND_GRSP_VEL_SRV, &BarrettHandInterface::handGraspVel, this); + hand_sprd_pos_srv = nh.advertiseService(HAND_SPRD_POS_SRV, &BarrettHandInterface::handSpreadPos, this); + hand_fngr_vel_srv = nh.advertiseService(HAND_SPRD_VEL_SRV, &BarrettHandInterface::handFingerVel, this); + + /* TODO Set the safety limits + * Move j3 to give room for hand initialization + */ + //Set up the BarrettHand joint state publisher + std::vector bhand_joints(bhand_jnts, bhand_jnts + 7); + js.name.resize(7); + js.name = bhand_joints; + js.position.resize(7); + + publishHandInfo(); + + + } +} From 54bcba2ee8665169d6fff34531e0ad9a9b638813 Mon Sep 17 00:00:00 2001 From: Hariharasudan Date: Tue, 21 Oct 2014 12:38:42 -0400 Subject: [PATCH 5/7] Fixed the namespace for the topics and services --- include/barrett_arm.h | 44 ++++++++++++++++++++---------- include/barrett_hand.h | 61 +++++++++++++++++++++++++++++++++++++----- src/libbarrett_ros.cpp | 48 ++++++++++++++++++++++++++------- 3 files changed, 123 insertions(+), 30 deletions(-) diff --git a/include/barrett_arm.h b/include/barrett_arm.h index ae213d6..22067f9 100644 --- a/include/barrett_arm.h +++ b/include/barrett_arm.h @@ -1,8 +1,29 @@ /* - * barrett_arm.h - * - * Created on: Oct 16, 2014 - * Author: robot + Copyright 2012 Barrett Technology + + This file is part of libbarrett_ros. + + This version of libbarrett_ros is free software: you can redistribute it + and/or modify it under the terms of the GNU General Public License as + published by the Free Software Foundation, either version 3 of the + License, or (at your option) any later version. + + This version of libbarrett_ros is distributed in the hope that it will be + useful, but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License along + with this version of libbarrett_ros. If not, see + . + + Barrett Technology holds all copyrights on libbarrett_ros. As the sole + copyright holder, Barrett reserves the right to release future versions + of libbarrett_ros under a different license. + + File: barrett_arm.h + Date: 15 October, 2014 + Author: Hariharasudan Malaichamee */ #ifndef BARRETT_ARM_H_ @@ -11,7 +32,7 @@ #include "ros/ros.h" #include #include -#include // For strtod() +#include #include #include @@ -24,21 +45,16 @@ #include #include -#include - using namespace barrett; -const int RATE = 200; //Execution rate in Hz +namespace barm{ -const std::string BARRETT_ARM_CONTROL_TOPIC = "barrett/arm/control"; -const std::string BARRETT_ARM_JS_TOPIC = "barrett/arm/joint_state"; -const std::string BARRETT_ARM_ES_TOPIC = "barrett/arm/endpoint_state"; +const int ARM_RATE = 200; //Execution rate in Hz const std::string ARM_CONTROL_TOPIC = "arm/control"; const std::string ARM_JS_TOPIC = "arm/joint_states"; const std::string ARM_ES_TOPIC = "arm/endpoint_state"; - const std::string jnt_names[] = {"j1", "j2", "j3", "j4", "j5", "j6", "j7"}; template @@ -162,7 +178,7 @@ void BarrettArmInterface::start(){ } //Set the loop rate at 200 Hz - ros::Rate loop_rate(RATE); + ros::Rate loop_rate(ARM_RATE); //Turn on the gravity compensation by default arm_wam->gravityCompensate(); @@ -177,5 +193,5 @@ void BarrettArmInterface::start(){ arm_wam->idle(); arm_pm->getSafetyModule()->waitForMode(SafetyModule::IDLE); } - +} //namespace #endif /* BARRETT_ARM_H_ */ diff --git a/include/barrett_hand.h b/include/barrett_hand.h index fb4a156..ffe9214 100644 --- a/include/barrett_hand.h +++ b/include/barrett_hand.h @@ -1,3 +1,34 @@ +/* + Copyright 2012 Barrett Technology + + This file is part of libbarrett_ros. + + This version of libbarrett_ros is free software: you can redistribute it + and/or modify it under the terms of the GNU General Public License as + published by the Free Software Foundation, either version 3 of the + License, or (at your option) any later version. + + This version of libbarrett_ros is distributed in the hope that it will be + useful, but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License along + with this version of libbarrett_ros. If not, see + . + + Barrett Technology holds all copyrights on libbarrett_ros. As the sole + copyright holder, Barrett reserves the right to release future versions + of libbarrett_ros under a different license. + + File: barrett_hand.h + Date: 15 October, 2014 + Author: Hariharasudan Malaichamee + */ + +#ifndef BARRETT_HAND_H_ +#define BARRETT_HAND_H_ + #include "ros/ros.h" // The file below provides access to the barrett::units namespace. #include @@ -20,11 +51,15 @@ using namespace barrett; -const int RATE = 200; //Execution rate in Hz -const char* bhand_jnts[] = {"inner_f1", "inner_f2", "inner_f3", "spread", "outer_f1", "outer_f2", "outer_f3"}; +namespace bhand{ +const int HAND_RATE = 200; //Execution rate in Hz +const char* bhand_jnts[] = {"inner_f1", "inner_f2", "inner_f3", "spread", "outer_f1", "outer_f2", "outer_f3"}; //Hand Joints' name + +//Publish topics const std::string HAND_JS_TOPIC = "hand/joint_states"; +//Service topics const std::string HAND_OPN_GRSP_SRV = "hand/open_grasp"; const std::string HAND_CLS_GRSP_SRV = "hand/close_grasp"; const std::string HAND_OPN_SPRD_SRV = "hand/open_spread"; @@ -36,18 +71,23 @@ const std::string HAND_GRSP_VEL_SRV = "hand/grasp_vel"; const std::string HAND_SPRD_POS_SRV = "hand/spread_pos"; const std::string HAND_SPRD_VEL_SRV = "hand/spread_vel"; +//Default Request and Response messages +std_srvs::Empty::Request def_req; +std_srvs::Empty::Response def_res; + template class BarrettHandInterface{ BARRETT_UNITS_TEMPLATE_TYPEDEFS(DOF); bool isInitialized; //Tracks if the hand is initialized - std_srvs::Empty::Request def_req; - std_srvs::Empty::Response def_res; + sensor_msgs::JointState js; systems::Wam* hand_wam; ProductManager* hand_pm; Hand* hand; + ForceTorqueSensor* fts; + ros::NodeHandle nh; ros::Publisher hand_js_pub; ros::ServiceServer hand_open_grsp_srv, hand_close_grsp_srv, hand_open_sprd_srv; @@ -69,7 +109,8 @@ class BarrettHandInterface{ bool publishHandInfo(); public: - BarrettHandInterface(ProductManager &pm, systems::Wam &wam): hand_wam(&wam), hand_pm(&pm), hand(NULL), isInitialized(false){}; + BarrettHandInterface(ProductManager &pm, systems::Wam &wam): hand_wam(&wam), hand_pm(&pm), hand(NULL), + isInitialized(false), fts(NULL){}; void start(); }; @@ -216,7 +257,7 @@ template bool BarrettHandInterface::publishHandInfo(){ //Set the loop rate at 200 Hz - ros::Rate loop_rate(RATE); + ros::Rate loop_rate(HAND_RATE); while (ros::ok()) { @@ -232,6 +273,10 @@ bool BarrettHandInterface::publishHandInfo(){ loop_rate.sleep(); } } + +/* + * Initializes the publishers, services and assigns the defaults + */ template void BarrettHandInterface::start(){ @@ -256,6 +301,7 @@ void BarrettHandInterface::start(){ /* TODO Set the safety limits * Move j3 to give room for hand initialization */ + //Set up the BarrettHand joint state publisher std::vector bhand_joints(bhand_jnts, bhand_jnts + 7); js.name.resize(7); @@ -264,6 +310,7 @@ void BarrettHandInterface::start(){ publishHandInfo(); - } } +} //namespace +#endif /* BARRETT_HAND_H_ */ diff --git a/src/libbarrett_ros.cpp b/src/libbarrett_ros.cpp index 106fa1b..24156ef 100644 --- a/src/libbarrett_ros.cpp +++ b/src/libbarrett_ros.cpp @@ -1,26 +1,56 @@ /* - * libbarrett_ros.cpp - * - * Created on: Oct 16, 2014 - * Author: Hariharasudan Malaichamee + Copyright 2012 Barrett Technology + + This file is part of libbarrett_ros. + + This version of libbarrett_ros is free software: you can redistribute it + and/or modify it under the terms of the GNU General Public License as + published by the Free Software Foundation, either version 3 of the + License, or (at your option) any later version. + + This version of libbarrett_ros is distributed in the hope that it will be + useful, but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License along + with this version of libbarrett_ros. If not, see + . + + Barrett Technology holds all copyrights on libbarrett_ros. As the sole + copyright holder, Barrett reserves the right to release future versions + of libbarrett_ros under a different license. + + File: libbarrett_ros.cpp + Date: 15 October, 2014 + Author: Hariharasudan Malaichamee */ + #include "ros/ros.h" #include #include #include -#include "../include/barrett_arm.h" +#include +#include using namespace barrett; template int wam_main(int argc, char** argv, ProductManager& pm, systems::Wam& wam) { - ros::init(argc, argv, "libbarrett_interface"); + ros::init(argc, argv, "libbarrett_ros"); + + barm::BarrettArmInterface barrett_arm( pm, wam); + bhand::BarrettHandInterface barrett_hand( pm, wam); - barrett_arm_interface barrett_arm( pm, wam); + //Create thread for the Barrett Arm + boost::thread arm_thread(&barm::BarrettArmInterface::start, &barrett_arm); - //Create thread for the individual products - boost::thread arm_thread(&barrett_arm_interface::start, &barrett_arm); + //Check if the hand is present and start a seperate thread for it + if(pm.foundHand()) { + boost::thread hand_thread(&bhand::BarrettHandInterface::start, &barrett_hand); + hand_thread.join(); + } arm_thread.join(); return 0; From dadf33aeb12ac92aa35d7f689af0159a7e4f5e10 Mon Sep 17 00:00:00 2001 From: Hariharasudan Date: Mon, 3 Nov 2014 11:11:05 -0500 Subject: [PATCH 6/7] Modified arm and hand --- include/barrett_arm.h | 118 +++++++++++++++++++++++++++++------------ include/barrett_hand.h | 1 - 2 files changed, 83 insertions(+), 36 deletions(-) diff --git a/include/barrett_arm.h b/include/barrett_arm.h index 22067f9..3c09477 100644 --- a/include/barrett_arm.h +++ b/include/barrett_arm.h @@ -36,9 +36,12 @@ #include #include -#include +#include +#include #include +#include +#include // The file below provides access to the barrett::units namespace. #include @@ -62,18 +65,18 @@ class BarrettArmInterface{ BARRETT_UNITS_TEMPLATE_TYPEDEFS(DOF); //WAM information for retrieval - jp_type jp; - jv_type jv; - jt_type jt; + jp_type jp_info, jp_cmd; + jv_type jv_info; + jt_type jt_info; - cp_type cp; - cv_type cv; - pose_type pt; - Eigen::Quaterniond qt; + cp_type cp_info; + cv_type cv_info; + pose_type pt_info; + Eigen::Quaterniond qt_info; //WAM information for publishing - sensor_msgs::JointState js; - wam_msgs::EndpointState es; + sensor_msgs::JointState js_info; + wam_msgs::EndpointState es_info; systems::Wam* arm_wam; ProductManager* arm_pm; @@ -81,8 +84,11 @@ class BarrettArmInterface{ ros::Subscriber arm_control_sub; ros::Publisher arm_js_pub, arm_es_pub; + bool controlGravity(wam_msgs::GravityComp::Request &, wam_msgs::GravityComp::Response &); void armControlModes(const wam_msgs::RTPosMode&); void armPublishInfo(); + bool armMoveHome(std_srvs::Empty::Request &, std_srvs::Empty::Response &); + bool armJointMoveTo(wam_msgs::JointMove::Request &, wam_msgs::JointMove::Response &); public: BarrettArmInterface(ProductManager &pm, systems::Wam &wam): arm_wam(&wam), arm_pm(&pm){}; @@ -90,32 +96,44 @@ class BarrettArmInterface{ }; +/* + * Service call to turn on/off the gravity as requested + */ +template +bool BarrettArmInterface::controlGravity(wam_msgs::GravityComp::Request &req, wam_msgs::GravityComp::Response &res){ + + arm_wam->gravityCompensate(req.gravity); + ROS_INFO("Gravity Compensation Request: %s", (req.gravity) ? "true" : "false"); + return true; +} + + template void BarrettArmInterface::armControlModes(const wam_msgs::RTPosMode& msg){ switch (msg.mode) { case wam_msgs::RTPosMode::JOINT_POSITION_CONTROL: - printf("Holding joint positions.\n"); + ROS_INFO("Holding joint positions."); arm_wam->moveTo(arm_wam->getJointPositions()); break; case wam_msgs::RTPosMode::CARTESIAN_POSITION_CONTROL: - printf("Holding tool position.\n"); + ROS_INFO("Holding tool position."); arm_wam->moveTo(arm_wam->getToolPosition()); break; case wam_msgs::RTPosMode::CARTESIAN_ORIENTATION_CONTROL: - printf("Holding tool orientation.\n"); + ROS_INFO("Holding tool orientation."); arm_wam->moveTo(arm_wam->getToolOrientation()); break; case wam_msgs::RTPosMode::CARTESIAN_POS_ORIENT_CONTROL: - printf("Holding both tool position and orientation.\n"); - arm_wam->moveTo(arm_wam->getToolOrientation()); + ROS_INFO("Holding both tool position and orientation."); + arm_wam->moveTo(arm_wam->getToolPose()); break; case wam_msgs::RTPosMode::GRAVITY_COMP: - std::cout << "Moving to home position: "<< std::endl; - arm_wam->moveHome(); + ROS_INFO("Gravity Compensation mode."); + arm_wam->gravityCompensate(); break; default: @@ -130,32 +148,62 @@ void BarrettArmInterface::armControlModes(const wam_msgs::RTPosMode& msg){ template void BarrettArmInterface::armPublishInfo(){ - jp = arm_wam->getJointPositions(); - jv = arm_wam->getJointVelocities(); - jt = arm_wam->getJointTorques(); + jp_info = arm_wam->getJointPositions(); + jv_info = arm_wam->getJointVelocities(); + jt_info = arm_wam->getJointTorques(); - cp = arm_wam->getToolPosition(); - cv = arm_wam->getToolVelocity(); - qt = arm_wam->getToolOrientation(); + cp_info = arm_wam->getToolPosition(); + cv_info = arm_wam->getToolVelocity(); + qt_info = arm_wam->getToolOrientation(); //Pack the Joint and Endpoint state messages with the updated values for(size_t i = 0; i < DOF; ++i){ - js.position[i] = jp[i]; - js.velocity[i] = jv[i]; - js.effort[i] = jt[i]; + js_info.position[i] = jp_info[i]; + js_info.velocity[i] = jv_info[i]; + js_info.effort[i] = jt_info[i]; if(i<3){ - es.positions[i] = cp[i]; - es.velocities[i] = cv[i]; + es_info.positions[i] = cp_info[i]; + es_info.velocities[i] = cv_info[i]; } } - es.orientation.x = qt.x(); - es.orientation.y = qt.y(); - es.orientation.z = qt.z(); - es.orientation.w = qt.w(); + es_info.orientation.x = qt_info.x(); + es_info.orientation.y = qt_info.y(); + es_info.orientation.z = qt_info.z(); + es_info.orientation.w = qt_info.w(); - arm_js_pub.publish(js); - arm_es_pub.publish(es); + arm_js_pub.publish(js_info); + arm_es_pub.publish(es_info); +} + +/* + * Moves the arm to its home position + */ +template +bool BarrettArmInterface::armMoveHome(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res){ + + ROS_INFO("Moving the arm to its home position."); + arm_wam->moveHome(); + return true; +} + +/* + * Moves the arm joints to the commanded position + */ +template +bool BarrettArmInterface::armJointMoveTo(wam_msgs::JointMove::Request &req, wam_msgs::JointMove::Response &res){ + + if (req.joints.size() != DOF) + { + ROS_INFO("Request Failed: %zu-DOF request received, must be %zu-DOF", req.joints.size(), DOF); + return false; + } + + ROS_INFO("Moving the arm joints to Commanded Joint Position"); + for (size_t i = 0; i < DOF; i++) + jp_cmd[i] = req.joints[i]; + arm_wam->moveTo(jp_cmd, false); + return true; } /* @@ -174,7 +222,7 @@ void BarrettArmInterface::start(){ //Initialize the Joint state publisher message with its default values for (size_t i=0; i - using namespace barrett; namespace bhand{ From 7dfc06e22d3bad0e5c22ee4f201d5faed49e8112 Mon Sep 17 00:00:00 2001 From: Hariharasudan Date: Mon, 3 Nov 2014 11:55:03 -0500 Subject: [PATCH 7/7] Fixed return and wreorder warnings --- include/barrett_hand.h | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/include/barrett_hand.h b/include/barrett_hand.h index e8742ba..dabb011 100644 --- a/include/barrett_hand.h +++ b/include/barrett_hand.h @@ -105,11 +105,11 @@ class BarrettHandInterface{ bool handGraspVel(wam_msgs::BHandGraspVel::Request &, wam_msgs::BHandGraspVel::Response &); bool handSpreadPos(wam_msgs::BHandSpreadPos::Request &, wam_msgs::BHandSpreadPos::Response &); bool handSpreadVel(wam_msgs::BHandSpreadVel::Request &, wam_msgs::BHandSpreadVel::Response &); - bool publishHandInfo(); + void publishHandInfo(); public: - BarrettHandInterface(ProductManager &pm, systems::Wam &wam): hand_wam(&wam), hand_pm(&pm), hand(NULL), - isInitialized(false), fts(NULL){}; + BarrettHandInterface(ProductManager &pm, systems::Wam &wam): isInitialized(false), hand_wam(&wam), hand_pm(&pm), + hand(NULL), fts(NULL){}; void start(); }; @@ -253,7 +253,7 @@ bool BarrettHandInterface::handSpreadVel(wam_msgs::BHandSpreadVel::Request //Function to publish the joint states of the hand at desired rate template -bool BarrettHandInterface::publishHandInfo(){ +void BarrettHandInterface::publishHandInfo(){ //Set the loop rate at 200 Hz ros::Rate loop_rate(HAND_RATE);