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/include/barrett_arm.h b/include/barrett_arm.h new file mode 100644 index 0000000..3c09477 --- /dev/null +++ b/include/barrett_arm.h @@ -0,0 +1,245 @@ +/* + 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_ +#define BARRETT_ARM_H_ + +#include "ros/ros.h" +#include +#include +#include + +#include +#include +#include + +#include +#include +#include +#include + +// The file below provides access to the barrett::units namespace. +#include +#include +#include + +using namespace barrett; + +namespace barm{ + +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 +class BarrettArmInterface{ + BARRETT_UNITS_TEMPLATE_TYPEDEFS(DOF); + + //WAM information for retrieval + jp_type jp_info, jp_cmd; + jv_type jv_info; + jt_type jt_info; + + cp_type cp_info; + cv_type cv_info; + pose_type pt_info; + Eigen::Quaterniond qt_info; + + //WAM information for publishing + sensor_msgs::JointState js_info; + wam_msgs::EndpointState es_info; + + systems::Wam* arm_wam; + ProductManager* arm_pm; + ros::NodeHandle nh; + 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){}; + void start(); + +}; + +/* + * 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: + ROS_INFO("Holding joint positions."); + arm_wam->moveTo(arm_wam->getJointPositions()); + break; + + case wam_msgs::RTPosMode::CARTESIAN_POSITION_CONTROL: + ROS_INFO("Holding tool position."); + arm_wam->moveTo(arm_wam->getToolPosition()); + break; + + case wam_msgs::RTPosMode::CARTESIAN_ORIENTATION_CONTROL: + ROS_INFO("Holding tool orientation."); + arm_wam->moveTo(arm_wam->getToolOrientation()); + break; + + case wam_msgs::RTPosMode::CARTESIAN_POS_ORIENT_CONTROL: + ROS_INFO("Holding both tool position and orientation."); + arm_wam->moveTo(arm_wam->getToolPose()); + break; + + case wam_msgs::RTPosMode::GRAVITY_COMP: + ROS_INFO("Gravity Compensation mode."); + arm_wam->gravityCompensate(); + break; + + default: + printf("Unrecognized option.\n"); + break; + } + } + +/* + * Publish the Joint and the endpoint states of the wam + */ +template +void BarrettArmInterface::armPublishInfo(){ + + jp_info = arm_wam->getJointPositions(); + jv_info = arm_wam->getJointVelocities(); + jt_info = arm_wam->getJointTorques(); + + 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_info.position[i] = jp_info[i]; + js_info.velocity[i] = jv_info[i]; + js_info.effort[i] = jt_info[i]; + + if(i<3){ + es_info.positions[i] = cp_info[i]; + es_info.velocities[i] = cv_info[i]; + } + } + 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_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; +} + +/* + * Initialize the subscribers and publishers. Initialize the messages with their default values + */ +template +void BarrettArmInterface::start(){ + + arm_control_sub = nh.subscribe(ARM_CONTROL_TOPIC, 1, &BarrettArmInterface::armControlModes, this); + + //Joint Publishers + arm_js_pub = nh.advertise(ARM_JS_TOPIC, 1); + + //Cartesian Publishers + arm_es_pub = nh.advertise(ARM_ES_TOPIC, 1); + + //Initialize the Joint state publisher message with its default values + for (size_t i=0; igravityCompensate(); + + while(ros::ok){ + armPublishInfo(); + ros::spinOnce(); + loop_rate.sleep(); + } + + //Idle the Wam + 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 new file mode 100644 index 0000000..dabb011 --- /dev/null +++ b/include/barrett_hand.h @@ -0,0 +1,315 @@ +/* + 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 +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +#include + +using namespace barrett; + +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"; +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"; + +//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 + + 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; + 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 &); + void publishHandInfo(); + +public: + BarrettHandInterface(ProductManager &pm, systems::Wam &wam): isInitialized(false), hand_wam(&wam), hand_pm(&pm), + hand(NULL), fts(NULL){}; + 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 +void BarrettHandInterface::publishHandInfo(){ + + //Set the loop rate at 200 Hz + ros::Rate loop_rate(HAND_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(); + } +} + +/* + * Initializes the publishers, services and assigns the defaults + */ +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(); + + } +} +} //namespace +#endif /* BARRETT_HAND_H_ */ diff --git a/src/barrett_arm.cpp b/src/barrett_arm.cpp deleted file mode 100644 index ce3c542..0000000 --- a/src/barrett_arm.cpp +++ /dev/null @@ -1,177 +0,0 @@ -/* - * 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, - */ - -#include "ros/ros.h" -#include -#include -#include // For strtod() - -#include -#include -#include - -#include - -// The file below provides access to the barrett::units namespace. -#include -#include -#include - -#include - -using namespace barrett; -const std::string BARRETT_ARM_CONTROL_TOPIC = "barrett/wam/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 jnt_names[] = {"j1", "j2", "j3", "j4", "j5", "j6", "j7"}; - -template -class barrett_arm_interface{ - BARRETT_UNITS_TEMPLATE_TYPEDEFS(DOF); - - //WAM information for retrieval - jp_type jp; - jv_type jv; - jt_type jt; - - cp_type cp; - cv_type cv; - pose_type pt; - Eigen::Quaterniond qt; - - //WAM information for publishing - sensor_msgs::JointState js; - wam_msgs::EndpointState es; - - systems::Wam * arm_wam; - 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); - -public: - barrett_arm_interface(ProductManager &pm, systems::Wam &wam); - -}; -template -void barrett_arm_interface::barrett_arm_control_modes(const wam_msgs::RTPosMode& msg){ - switch (msg.mode) { - case wam_msgs::RTPosMode::JOINT_POSITION_CONTROL: - printf("Holding joint positions.\n"); - arm_wam->moveTo(arm_wam->getJointPositions()); - break; - - case wam_msgs::RTPosMode::CARTESIAN_POSITION_CONTROL: - printf("Holding tool position.\n"); - arm_wam->moveTo(arm_wam->getToolPosition()); - break; - - case wam_msgs::RTPosMode::CARTESIAN_ORIENTATION_CONTROL: - printf("Holding tool orientation.\n"); - 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()); - break; - - case wam_msgs::RTPosMode::GRAVITY_COMP: - std::cout << "Moving to home position: "<< std::endl; - arm_wam->moveHome(); - break; - - default: - printf("Unrecognized option.\n"); - break; - } - } - -/* - * Publish the Joint and the endpoint states of the wam - */ -template -void barrett_arm_interface::publish_barrett_arm_info(systems::Wam &wam){ - - jp=wam.getJointPositions(); - jv=wam.getJointVelocities(); - jt=wam.getJointTorques(); - - cp=wam.getToolPosition(); - cv=wam.getToolVelocity(); - qt = 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]; - - if(i<3){ - es.positions[i] = cp[i]; - es.velocities[i] = cv[i]; - } - } - es.orientation.x = qt.x(); - es.orientation.y = qt.y(); - es.orientation.z = qt.z(); - es.orientation.w = qt.w(); - - barrett_arm_js_pub.publish(js); - barrett_arm_es_pub.publish(es); -} - -/* - * 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){ - - barrett_arm_control_sub = nh.subscribe(BARRETT_ARM_CONTROL_TOPIC, 1, &barrett_arm_interface::barrett_arm_control_modes, this); - - //Joint Publishers - barrett_arm_js_pub = nh.advertise(BARRETT_ARM_JS_TOPIC, 1); - - //Cartesian Publishers - barrett_arm_es_pub = nh.advertise(BARRETT_ARM_ES_TOPIC, 1); - - //Initialize the Joint state publisher message with its default values - for (size_t i=0; iwaitForMode(SafetyModule::IDLE); -} - -template -int wam_main(int argc, char** argv, ProductManager& pm, systems::Wam& wam) { - - barrett_arm_interface w_if(pm, wam); - - return 0; -} - - - - 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; - } -} diff --git a/src/libbarrett_ros.cpp b/src/libbarrett_ros.cpp new file mode 100644 index 0000000..24156ef --- /dev/null +++ b/src/libbarrett_ros.cpp @@ -0,0 +1,57 @@ +/* + 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 + +using namespace barrett; +template +int wam_main(int argc, char** argv, ProductManager& pm, systems::Wam& wam) { + + ros::init(argc, argv, "libbarrett_ros"); + + barm::BarrettArmInterface barrett_arm( pm, wam); + bhand::BarrettHandInterface barrett_hand( pm, wam); + + //Create thread for the Barrett Arm + boost::thread arm_thread(&barm::BarrettArmInterface::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; +}