From: Walter Fetter Lages Date: Mon, 17 Dec 2018 19:17:34 +0000 (-0200) Subject: Change boost::thread to std::thread in wam_node_sim.cpp. X-Git-Url: http://git.ece.ufrgs.br/?a=commitdiff_plain;h=ff7c92e6836ca54dd15291dfbf9e57bf08830e8e;p=ufrgs_wam.git Change boost::thread to std::thread in wam_node_sim.cpp. --- diff --git a/wam_node_sim/src/hand.cpp.orig b/wam_node_sim/src/hand.cpp.orig deleted file mode 100644 index a0fda8c..0000000 --- a/wam_node_sim/src/hand.cpp.orig +++ /dev/null @@ -1,280 +0,0 @@ -/** - * Copyright 2009-2014 Barrett Technology - * - * This file is part of libbarrett. - * - * This version of libbarrett is free software: you can redistribute it - * and/or modify it under the terms of the GNU General Public License as - * published by the Free Software Foundation, either version 3 of the - * License, or (at your option) any later version. - * - * This version of libbarrett 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. If not, see - * . - * - * - * Barrett Technology Inc. - * 73 Chapel Street - * Newton, MA 02458 - * - */ - -/** - * @file hand.cpp - * @date 11/09/2010 - * @author Dan Cody - * - */ - -#include -#include -#include -#include - -#include - -#include -#include -#include -#include -#include -#include -#include -#include - - -namespace barrett { - - -const enum Puck::Property Hand::props[] = { Puck::HOLD, Puck::CMD, Puck::MODE, Puck::P, Puck::T, Puck::SG }; - -/** Hand Constructor */ -Hand::Hand(const std::vector& _pucks) : - MultiPuckProduct(DOF, _pucks, PuckGroup::BGRP_HAND, props, sizeof(props)/sizeof(props[0]), "Hand::Hand()"), - hasFtt(false), hasTact(false), useSecondaryEncoders(true), encoderTmp(DOF), primaryEncoder(DOF, 0), secondaryEncoder(DOF, 0), ftt(DOF, 0), tactilePucks() -{ - // Check for TACT and FingertipTorque options. - int numFtt = 0; - for (size_t i = 0; i < DOF; ++i) { - if (pucks[i]->hasOption(Puck::RO_Strain)) { - ++numFtt; - hasFtt = true; - } - } - logMessage(" Found %d Fingertip torque sensors") % numFtt; - - bool tactError = false; - for (size_t i = 0; i < DOF; ++i) { - if (pucks[i]->hasOption(Puck::RO_Tact)) { - try { - // The TactilePuck ctor might throw if there was an initialization error - tactilePucks.push_back(new TactilePuck(pucks[i])); - hasTact = true; - } catch (std::runtime_error e) { - tactError = true; - } - } - } - logMessage(" Found %d Tactile arrays") % tactilePucks.size(); - if (tactError) { - logMessage(" Initialization error! Disabling Tactile arrays"); - hasTact = false; - } - - // record HOLD values - group.getProperty(Puck::HOLD, holds); - - - // For the fingers - for (size_t i = 0; i < DOF - 1; ++i) { - j2pp[i] = motorPucks[i].getCountsPerRad() * J2_RATIO; - j2pt[i] = motorPucks[i].getIpnm() / J2_RATIO; - } - // For the spread - j2pp[SPREAD_INDEX] = motorPucks[SPREAD_INDEX].getCountsPerRad() * SPREAD_RATIO; - j2pt[SPREAD_INDEX] = motorPucks[SPREAD_INDEX].getIpnm() / SPREAD_RATIO; -} -/** Hand Destructor */ -Hand::~Hand() -{ - detail::purge(tactilePucks); -} -/** initialize Method */ -void Hand::initialize() const -{ - for (size_t i = 0; i < DOF-1; ++i) { - pucks[i]->setProperty(Puck::CMD, CMD_HI); - } - waitUntilDoneMoving(); - - pucks[SPREAD_INDEX]->setProperty(Puck::CMD, CMD_HI); - waitUntilDoneMoving(); -} -/** doneMoving Method */ -bool Hand::doneMoving(unsigned int whichDigits, bool realtime) const -{ - int modes[DOF]; - - // TODO(dc): Avoid asking for modes from the Pucks we don't care about. - group.getProperty(Puck::MODE, modes, realtime); - - for (size_t i = 0; i < DOF; ++i) { - if ( - digitsInclude(whichDigits, i) && - modes[i] != MotorPuck::MODE_IDLE && - (modes[i] != MotorPuck::MODE_PID || holds[i] == 0) - ) - { - return false; - } - } - return true; -} -/** waitUntilDoneMoving Method prevents any subsequent actions until finger movement is completed. */ -void Hand::waitUntilDoneMoving(unsigned int whichDigits, double period_s) const -{ - while ( !doneMoving(whichDigits) ) { - btsleep(period_s); - } -} -/** open Method */ -void Hand::open(unsigned int whichDigits, bool blocking) const { - setProperty(whichDigits, Puck::CMD, CMD_OPEN); - blockIf(blocking, whichDigits); -} -/** close Method */ -void Hand::close(unsigned int whichDigits, bool blocking) const { - setProperty(whichDigits, Puck::CMD, CMD_CLOSE); - blockIf(blocking, whichDigits); -} -/** trapezoidalMove Method */ -void Hand::trapezoidalMove(const jp_type& jp, unsigned int whichDigits, bool blocking) const -{ - setProperty(whichDigits, Puck::E, j2pp.cwise() * jp); - setProperty(whichDigits, Puck::MODE, MotorPuck::MODE_TRAPEZOIDAL); - blockIf(blocking, whichDigits); -} -/** velocityMove Method */ -void Hand::velocityMove(const jv_type& jv, unsigned int whichDigits) const -{ - // Convert to counts/millisecond - setProperty(whichDigits, Puck::V, (j2pp.cwise() * jv) / 1000.0); - setProperty(whichDigits, Puck::MODE, MotorPuck::MODE_VELOCITY); -} - -/** setPositionMode Method */ -void Hand::setPositionMode(unsigned int whichDigits) const { - setProperty(whichDigits, Puck::MODE, MotorPuck::MODE_PID); -} -/** setPositionCommand Method */ -void Hand::setPositionCommand(const jp_type& jp, unsigned int whichDigits) const -{ - setProperty(whichDigits, Puck::P, j2pp.cwise() * jp); -} -/** setTorqueMode Method */ -void Hand::setTorqueMode(unsigned int whichDigits) const { - setProperty(whichDigits, Puck::MODE, MotorPuck::MODE_TORQUE); -} -/** setTorqueCommand Method */ -void Hand::setTorqueCommand(const jt_type& jt, unsigned int whichDigits) const -{ - pt = j2pt.cwise() * jt; - if (whichDigits == WHOLE_HAND) { - MotorPuck::sendPackedTorques(pucks[0]->getBus(), group.getId(), Puck::T, pt.data(), DOF); - } else { - setProperty(whichDigits, Puck::T, pt); - } -} -/** update Method */ -void Hand::update(unsigned int sensors, bool realtime) -{ - // Do we need to lock? - boost::unique_lock ul(bus.getMutex(), boost::defer_lock); - if (realtime) { - ul.lock(); - } - - - // Send requests - if (sensors & S_POSITION) { - group.sendGetPropertyRequest(group.getPropertyId(Puck::P)); - } - if (hasFingertipTorqueSensors() && sensors & S_FINGERTIP_TORQUE) { - group.sendGetPropertyRequest(group.getPropertyId(Puck::SG)); - } - if (hasTactSensors() && sensors & S_TACT_FULL) { - group.setProperty(Puck::TACT, TactilePuck::FULL_FORMAT); - } - - - // Receive replies - if (sensors & S_POSITION) { - group.receiveGetPropertyReply >(group.getPropertyId(Puck::P), encoderTmp.data(), realtime); - - for (size_t i = 0; i < DOF; ++i) { - primaryEncoder[i] = encoderTmp[i].get<0>(); - secondaryEncoder[i] = encoderTmp[i].get<1>(); - } - - // For the fingers - for (size_t i = 0; i < DOF-1; ++i) { - // If we got a reading from the secondary encoder and it's enabled... - if (useSecondaryEncoders && secondaryEncoder[i] != std::numeric_limits::max()) { - innerJp[i] = motorPucks[i].counts2rad(secondaryEncoder[i]) / J2_ENCODER_RATIO; - outerJp[i] = motorPucks[i].counts2rad(primaryEncoder[i]) * (1.0/J2_RATIO + 1.0/J3_RATIO) - innerJp[i]; - } else { - // These calculations are only valid before breakaway! - innerJp[i] = motorPucks[i].counts2rad(primaryEncoder[i]) / J2_RATIO; - outerJp[i] = innerJp[i] * J2_RATIO / J3_RATIO; - } - } - - // For the spread - innerJp[SPREAD_INDEX] = outerJp[SPREAD_INDEX] = motorPucks[SPREAD_INDEX].counts2rad(primaryEncoder[SPREAD_INDEX]) / SPREAD_RATIO; - } - if (hasFingertipTorqueSensors() && sensors & S_FINGERTIP_TORQUE) { - group.receiveGetPropertyReply(group.getPropertyId(Puck::SG), ftt.data(), realtime); - } - if (hasTactSensors() && sensors & S_TACT_FULL) { - for (size_t i = 0; i < tactilePucks.size(); ++i) { - tactilePucks[i]->receiveFull(realtime); - } - } -} - -/** */ -void Hand::setProperty(unsigned int whichDigits, enum Puck::Property prop, int value) const -{ - if (whichDigits == WHOLE_HAND) { - group.setProperty(prop, value); - } else { - for (size_t i = 0; i < DOF; ++i) { - if (digitsInclude(whichDigits, i)) { - pucks[i]->setProperty(prop, value); - } - } - } -} -/** setProperty Method */ -void Hand::setProperty(unsigned int whichDigits, enum Puck::Property prop, const v_type& values) const -{ - for (size_t i = 0; i < DOF; ++i) { - if (digitsInclude(whichDigits, i)) { - pucks[i]->setProperty(prop, values[i]); - } - } -} -/** blockIf Method */ -void Hand::blockIf(bool blocking, unsigned int whichDigits) const { - if (blocking) { - waitUntilDoneMoving(whichDigits); - } -} - - -} diff --git a/wam_node_sim/src/wam_node.cpp.orig b/wam_node_sim/src/wam_node.cpp.orig deleted file mode 100644 index 866ca0b..0000000 --- a/wam_node_sim/src/wam_node.cpp.orig +++ /dev/null @@ -1,908 +0,0 @@ -/* - Copyright 2012 Barrett Technology - - This file is part of barrett-ros-pkg. - - This version of barrett-ros-pkg is free software: you can redistribute it - and/or modify it under the terms of the GNU General Public License as - published by the Free Software Foundation, either version 3 of the - License, or (at your option) any later version. - - This version of barrett-ros-pkg is distributed in the hope that it will be - useful, but WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - GNU General Public License for more details. - - You should have received a copy of the GNU General Public License along - with this version of barrett-ros-pkg. If not, see - . - - Barrett Technology holds all copyrights on barrett-ros-pkg. As the sole - copyright holder, Barrett reserves the right to release future versions - of barrett-ros-pkg under a different license. - - File: wam_node.cpp - Date: 5 June, 2012 - Author: Kyle Maroney - */ - -#include -#include - -#include // BarrettHand threading -#include - -#include "ros/ros.h" -#include "tf/transform_datatypes.h" - -#include "wam_msgs/RTJointPos.h" -#include "wam_msgs/RTJointVel.h" -#include "wam_msgs/RTCartPos.h" -#include "wam_msgs/RTCartVel.h" -#include "wam_msgs/RTOrtnPos.h" -#include "wam_msgs/RTOrtnVel.h" -#include "wam_srvs/GravityComp.h" -#include "wam_srvs/Hold.h" -#include "wam_srvs/JointMove.h" -#include "wam_srvs/PoseMove.h" -#include "wam_srvs/CartPosMove.h" -#include "wam_srvs/OrtnMove.h" -#include "wam_srvs/BHandFingerPos.h" -#include "wam_srvs/BHandGraspPos.h" -#include "wam_srvs/BHandSpreadPos.h" -#include "wam_srvs/BHandFingerVel.h" -#include "wam_srvs/BHandGraspVel.h" -#include "wam_srvs/BHandSpreadVel.h" -#include "std_srvs/Empty.h" -#include "sensor_msgs/JointState.h" -#include "geometry_msgs/PoseStamped.h" - -#include -#include -#include -#include -#include -#include -#include - -static const int PUBLISH_FREQ = 250; // Default Control Loop / Publishing Frequency -static const int BHAND_PUBLISH_FREQ = 5; // Publishing Frequency for the BarretHand -static const double SPEED = 0.03; // Default Cartesian Velocity - -using namespace barrett; - -//Creating a templated multiplier for our real-time computation -template - class Multiplier : public systems::System, public systems::SingleOutput - { - public: - Input input1; - public: - Input input2; - - public: - Multiplier(std::string sysName = "Multiplier") : - systems::System(sysName), systems::SingleOutput(this), input1(this), input2(this) - { - } - virtual ~Multiplier() - { - mandatoryCleanUp(); - } - - protected: - OutputType data; - virtual void operate() - { - data = input1.getValue() * input2.getValue(); - this->outputValue->setData(&data); - } - - private: - DISALLOW_COPY_AND_ASSIGN(Multiplier); - - public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - }; - -//Creating a templated converter from Roll, Pitch, Yaw to Quaternion for real-time computation -class ToQuaternion : public systems::SingleIO::type, Eigen::Quaterniond> -{ -public: - Eigen::Quaterniond outputQuat; - -public: - ToQuaternion(std::string sysName = "ToQuaternion") : - systems::SingleIO::type, Eigen::Quaterniond>(sysName) - { - } - virtual ~ToQuaternion() - { - mandatoryCleanUp(); - } - -protected: - btQuaternion q; - virtual void operate() - { - const math::Vector<3>::type &inputRPY = input.getValue(); - q.setEulerZYX(inputRPY[2], inputRPY[1], inputRPY[0]); - outputQuat.x() = q.getX(); - outputQuat.y() = q.getY(); - outputQuat.z() = q.getZ(); - outputQuat.w() = q.getW(); - this->outputValue->setData(&outputQuat); - } - -private: - DISALLOW_COPY_AND_ASSIGN(ToQuaternion); - -public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW -}; - -//Simple Function for converting Quaternion to RPY -math::Vector<3>::type toRPY(Eigen::Quaterniond inquat) -{ - math::Vector<3>::type newRPY; - btQuaternion q(inquat.x(), inquat.y(), inquat.z(), inquat.w()); - btMatrix3x3(q).getEulerZYX(newRPY[2], newRPY[1], newRPY[0]); - return newRPY; -} - -//WamNode Class -template - class WamNode - { - BARRETT_UNITS_TEMPLATE_TYPEDEFS(DOF); - protected: - bool cart_vel_status, ortn_vel_status, jnt_vel_status; - bool jnt_pos_status, cart_pos_status, ortn_pos_status, new_rt_cmd; - double cart_vel_mag, ortn_vel_mag; - systems::Wam& wam; - Hand* hand; - jp_type jp, jp_cmd, jp_home; - jp_type rt_jp_cmd, rt_jp_rl; - jv_type rt_jv_cmd; - cp_type cp_cmd, rt_cv_cmd; - cp_type rt_cp_cmd, rt_cp_rl; - Eigen::Quaterniond ortn_cmd, rt_op_cmd, rt_op_rl; - pose_type pose_cmd; - math::Vector<3>::type rt_ortn_cmd; - systems::ExposedOutput orientationSetPoint, current_ortn; - systems::ExposedOutput cart_dir, current_cart_pos, cp_track; - systems::ExposedOutput::type> rpy_cmd, current_rpy_ortn; - systems::ExposedOutput jv_track; - systems::ExposedOutput jp_track; - systems::TupleGrouper rt_pose_cmd; - systems::Summer cart_pos_sum; - systems::Summer::type> ortn_cmd_sum; - systems::Ramp ramp; - systems::RateLimiter jp_rl; - systems::RateLimiter cp_rl; - Multiplier mult_linear; - Multiplier::type, math::Vector<3>::type> mult_angular; - ToQuaternion to_quat, to_quat_print; - Eigen::Quaterniond ortn_print; - ros::Time last_cart_vel_msg_time, last_ortn_vel_msg_time, last_jnt_vel_msg_time; - ros::Time last_jnt_pos_msg_time, last_cart_pos_msg_time, last_ortn_pos_msg_time; - ros::Duration rt_msg_timeout; - - //Subscribed Topics - wam_msgs::RTCartVel cart_vel_cmd; - wam_msgs::RTOrtnVel ortn_vel_cmd; - - //Subscribers - ros::Subscriber cart_vel_sub; - ros::Subscriber ortn_vel_sub; - ros::Subscriber jnt_vel_sub; - ros::Subscriber jnt_pos_sub; - ros::Subscriber cart_pos_sub; - ros::Subscriber ortn_pos_sub; - - //Published Topics - sensor_msgs::JointState wam_joint_state, bhand_joint_state; - geometry_msgs::PoseStamped wam_pose; - - //Publishers - ros::Publisher wam_joint_state_pub, bhand_joint_state_pub, wam_pose_pub; - - //Services - ros::ServiceServer gravity_srv, go_home_srv, hold_jpos_srv, hold_cpos_srv; - ros::ServiceServer hold_ortn_srv, joint_move_srv, pose_move_srv; - ros::ServiceServer cart_move_srv, ortn_move_srv, hand_close_srv; - ros::ServiceServer hand_open_grsp_srv, hand_close_grsp_srv, hand_open_sprd_srv; - ros::ServiceServer hand_close_sprd_srv, hand_fngr_pos_srv, hand_fngr_vel_srv; - ros::ServiceServer hand_grsp_pos_srv, hand_grsp_vel_srv, hand_sprd_pos_srv; - ros::ServiceServer hand_sprd_vel_srv; - - public: - WamNode(systems::Wam& wam_) : - wam(wam_), hand(NULL), ramp(NULL, SPEED) - { - } - void - init(ProductManager& pm); - - ~WamNode() - { - } - - bool - gravity(wam_srvs::GravityComp::Request &req, wam_srvs::GravityComp::Response &res); - bool - goHome(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res); - bool - holdJPos(wam_srvs::Hold::Request &req, wam_srvs::Hold::Response &res); - bool - holdCPos(wam_srvs::Hold::Request &req, wam_srvs::Hold::Response &res); - bool - holdOrtn(wam_srvs::Hold::Request &req, wam_srvs::Hold::Response &res); - bool - jointMove(wam_srvs::JointMove::Request &req, wam_srvs::JointMove::Response &res); - bool - poseMove(wam_srvs::PoseMove::Request &req, wam_srvs::PoseMove::Response &res); - bool - cartMove(wam_srvs::CartPosMove::Request &req, wam_srvs::CartPosMove::Response &res); - bool - ortnMove(wam_srvs::OrtnMove::Request &req, wam_srvs::OrtnMove::Response &res); - bool - handOpenGrasp(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res); - bool - handCloseGrasp(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res); - bool - handOpenSpread(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res); - bool - handCloseSpread(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res); - bool - handFingerPos(wam_srvs::BHandFingerPos::Request &req, wam_srvs::BHandFingerPos::Response &res); - bool - handGraspPos(wam_srvs::BHandGraspPos::Request &req, wam_srvs::BHandGraspPos::Response &res); - bool - handSpreadPos(wam_srvs::BHandSpreadPos::Request &req, wam_srvs::BHandSpreadPos::Response &res); - bool - handFingerVel(wam_srvs::BHandFingerVel::Request &req, wam_srvs::BHandFingerVel::Response &res); - bool - handGraspVel(wam_srvs::BHandGraspVel::Request &req, wam_srvs::BHandGraspVel::Response &res); - bool - handSpreadVel(wam_srvs::BHandSpreadVel::Request &req, wam_srvs::BHandSpreadVel::Response &res); - void - cartVelCB(const wam_msgs::RTCartVel::ConstPtr& msg); - void - ortnVelCB(const wam_msgs::RTOrtnVel::ConstPtr& msg); - void - jntVelCB(const wam_msgs::RTJointVel::ConstPtr& msg); - void - jntPosCB(const wam_msgs::RTJointPos::ConstPtr& msg); - void - cartPosCB(const wam_msgs::RTCartPos::ConstPtr& msg); - void - publishWam(ProductManager& pm); - void - publishHand(void); - void - updateRT(ProductManager& pm); - }; - -// Templated Initialization Function -template - void WamNode::init(ProductManager& pm) - { - ros::NodeHandle n_("wam"); // WAM specific nodehandle - ros::NodeHandle nh_("bhand"); // BarrettHand specific nodehandle - - //Setting up real-time command timeouts and initial values - cart_vel_status = false; //Bool for determining cartesian velocity real-time state - ortn_vel_status = false; //Bool for determining orientation velocity real-time state - new_rt_cmd = false; //Bool for determining if a new real-time message was received - rt_msg_timeout.fromSec(0.3); //rt_status will be determined false if rt message is not received in specified time - cart_vel_mag = SPEED; //Setting default cartesian velocity magnitude to SPEED - ortn_vel_mag = SPEED; - pm.getExecutionManager()->startManaging(ramp); //starting ramp manager - - ROS_INFO(" \n %zu-DOF WAM", DOF); - jp_home = wam.getJointPositions(); - - if (pm.foundHand()) //Does the following only if a BarrettHand is present - { - std::cout << "Barrett Hand" << std::endl; - hand = pm.getHand(); - - // Adjust the torque limits to allow for BarrettHand movements at extents - pm.getSafetyModule()->setTorqueLimit(3.0); - - // Move j3 in order to give room for hand initialization - jp_type jp_init = wam.getJointPositions(); - jp_init[3] -= 0.35; - usleep(500000); - wam.moveTo(jp_init); - - usleep(500000); - hand->initialize(); - hand->update(); - - //Publishing the following topics only if there is a BarrettHand present - bhand_joint_state_pub = nh_.advertise < sensor_msgs::JointState > ("joint_states", 1); // bhand/joint_states - - //Advertise the following services only if there is a BarrettHand present - hand_open_grsp_srv = nh_.advertiseService("open_grasp", &WamNode::handOpenGrasp, this); // bhand/open_grasp - hand_close_grsp_srv = nh_.advertiseService("close_grasp", &WamNode::handCloseGrasp, this); // bhand/close_grasp - hand_open_sprd_srv = nh_.advertiseService("open_spread", &WamNode::handOpenSpread, this); // bhand/open_spread - hand_close_sprd_srv = nh_.advertiseService("close_spread", &WamNode::handCloseSpread, this); // bhand/close_spread - hand_fngr_pos_srv = nh_.advertiseService("finger_pos", &WamNode::handFingerPos, this); // bhand/finger_pos - hand_grsp_pos_srv = nh_.advertiseService("grasp_pos", &WamNode::handGraspPos, this); // bhand/grasp_pos - hand_sprd_pos_srv = nh_.advertiseService("spread_pos", &WamNode::handSpreadPos, this); // bhand/spread_pos - hand_fngr_vel_srv = nh_.advertiseService("finger_vel", &WamNode::handFingerVel, this); // bhand/finger_vel - hand_grsp_vel_srv = nh_.advertiseService("grasp_vel", &WamNode::handGraspVel, this); // bhand/grasp_vel - hand_sprd_vel_srv = nh_.advertiseService("spread_vel", &WamNode::handSpreadVel, this); // bhand/spread_vel - - //Set up the BarrettHand joint state publisher - const char* bhand_jnts[] = {"inner_f1", "inner_f2", "inner_f3", "spread", "outer_f1", "outer_f2", "outer_f3"}; - std::vector < std::string > bhand_joints(bhand_jnts, bhand_jnts + 7); - bhand_joint_state.name.resize(7); - bhand_joint_state.name = bhand_joints; - bhand_joint_state.position.resize(7); - } - - wam.gravityCompensate(true); // Turning on Gravity Compenstation by Default when starting the WAM Node - - //Setting up WAM joint state publisher - const char* wam_jnts[] = {"wam_j1", "wam_j2", "wam_j3", "wam_j4", "wam_j5", "wam_j6", "wam_j7"}; - std::vector < std::string > wam_joints(wam_jnts, wam_jnts + 7); - wam_joint_state.name = wam_joints; - wam_joint_state.name.resize(DOF); - wam_joint_state.position.resize(DOF); - wam_joint_state.velocity.resize(DOF); - wam_joint_state.effort.resize(DOF); - - //Publishing the following rostopics - wam_joint_state_pub = n_.advertise < sensor_msgs::JointState > ("joint_states", 1); // wam/joint_states - wam_pose_pub = n_.advertise < geometry_msgs::PoseStamped > ("pose", 1); // wam/pose - - //Subscribing to the following rostopics - cart_vel_sub = n_.subscribe("cart_vel_cmd", 1, &WamNode::cartVelCB, this); // wam/cart_vel_cmd - ortn_vel_sub = n_.subscribe("ortn_vel_cmd", 1, &WamNode::ortnVelCB, this); // wam/ortn_vel_cmd - jnt_vel_sub = n_.subscribe("jnt_vel_cmd", 1, &WamNode::jntVelCB, this); // wam/jnt_vel_cmd - jnt_pos_sub = n_.subscribe("jnt_pos_cmd", 1, &WamNode::jntPosCB, this); // wam/jnt_pos_cmd - cart_pos_sub = n_.subscribe("cart_pos_cmd", 1, &WamNode::cartPosCB, this); // wam/cart_pos_cmd - - //Advertising the following rosservices - gravity_srv = n_.advertiseService("gravity_comp", &WamNode::gravity, this); // wam/gravity_comp - go_home_srv = n_.advertiseService("go_home", &WamNode::goHome, this); // wam/go_home - hold_jpos_srv = n_.advertiseService("hold_joint_pos", &WamNode::holdJPos, this); // wam/hold_joint_pos - hold_cpos_srv = n_.advertiseService("hold_cart_pos", &WamNode::holdCPos, this); // wam/hold_cart_pos - hold_ortn_srv = n_.advertiseService("hold_ortn", &WamNode::holdOrtn, this); // wam/hold_ortn - joint_move_srv = n_.advertiseService("joint_move", &WamNode::jointMove, this); // wam/joint_move - pose_move_srv = n_.advertiseService("pose_move", &WamNode::poseMove, this); // wam/pose_move - cart_move_srv = n_.advertiseService("cart_move", &WamNode::cartMove, this); // wam/cart_pos_move - ortn_move_srv = n_.advertiseService("ortn_move", &WamNode::ortnMove, this); // wam/ortn_move - - } - -// gravity_comp service callback -template - bool WamNode::gravity(wam_srvs::GravityComp::Request &req, wam_srvs::GravityComp::Response &res) - { - wam.gravityCompensate(req.gravity); - ROS_INFO("Gravity Compensation Request: %s", (req.gravity) ? "true" : "false"); - return true; - } - -// goHome Function for sending the WAM safely back to its home starting position. -template - bool WamNode::goHome(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res) - { - ROS_INFO("Returning to Home Position"); - - if (hand != NULL) - { - hand->open(Hand::GRASP, true); - hand->close(Hand::SPREAD, true); - } - for (size_t i = 0; i < DOF; i++) - jp_cmd[i] = 0.0; - wam.moveTo(jp_cmd, true); - jp_home[3] -= 0.3; - wam.moveTo(jp_home, true); - jp_home[3] += 0.3; - wam.moveTo(jp_home, true); - return true; - } - -//Function to hold WAM Joint Positions -template - bool WamNode::holdJPos(wam_srvs::Hold::Request &req, wam_srvs::Hold::Response &res) - { - ROS_INFO("Joint Position Hold request: %s", (req.hold) ? "true" : "false"); - - if (req.hold) - wam.moveTo(wam.getJointPositions()); - else - wam.idle(); - return true; - } - -//Function to hold WAM end effector Cartesian Position -template - bool WamNode::holdCPos(wam_srvs::Hold::Request &req, wam_srvs::Hold::Response &res) - { - ROS_INFO("Cartesian Position Hold request: %s", (req.hold) ? "true" : "false"); - - if (req.hold) - wam.moveTo(wam.getToolPosition()); - else - wam.idle(); - return true; - } - -//Function to hold WAM end effector Orientation -template - bool WamNode::holdOrtn(wam_srvs::Hold::Request &req, wam_srvs::Hold::Response &res) - { - ROS_INFO("Orientation Hold request: %s", (req.hold) ? "true" : "false"); - - if (req.hold) - { - orientationSetPoint.setValue(wam.getToolOrientation()); - wam.trackReferenceSignal(orientationSetPoint.output); - } - else - wam.idle(); - return true; - } - -//Function to command a joint space move to the WAM -template - bool WamNode::jointMove(wam_srvs::JointMove::Request &req, wam_srvs::JointMove::Response &res) - { - if (req.joints.size() != DOF) - { - ROS_INFO("Request Failed: %zu-DOF request received, must be %zu-DOF", req.joints.size(), DOF); - return false; - } - ROS_INFO("Moving Robot to Commanded Joint Pose"); - for (size_t i = 0; i < DOF; i++) - jp_cmd[i] = req.joints[i]; - wam.moveTo(jp_cmd, false); - return true; - } - -//Function to command a pose move to the WAM -template - bool WamNode::poseMove(wam_srvs::PoseMove::Request &req, wam_srvs::PoseMove::Response &res) - { - ROS_INFO("Moving Robot to Commanded Pose"); - - cp_cmd[0] = req.pose.position.x; - cp_cmd[1] = req.pose.position.y; - cp_cmd[2] = req.pose.position.z; - ortn_cmd.x() = req.pose.orientation.x; - ortn_cmd.y() = req.pose.orientation.y; - ortn_cmd.z() = req.pose.orientation.z; - ortn_cmd.w() = req.pose.orientation.w; - - pose_cmd = boost::make_tuple(cp_cmd, ortn_cmd); - - //wam.moveTo(pose_cmd, false); //(TODO:KM Update Libbarrett API for Pose Moves) - ROS_INFO("Pose Commands for WAM not yet supported by API"); - return false; - } - -//Function to command a cartesian move to the WAM -template - bool WamNode::cartMove(wam_srvs::CartPosMove::Request &req, wam_srvs::CartPosMove::Response &res) - { - ROS_INFO("Moving Robot to Commanded Cartesian Position"); - - for (int i = 0; i < 3; i++) - cp_cmd[i] = req.position[i]; - wam.moveTo(cp_cmd, false); - return true; - } - -//Function to command an orientation move to the WAM -template - bool WamNode::ortnMove(wam_srvs::OrtnMove::Request &req, wam_srvs::OrtnMove::Response &res) - { - ROS_INFO("Moving Robot to Commanded End Effector Orientation"); - - ortn_cmd.x() = req.orientation[0]; - ortn_cmd.y() = req.orientation[1]; - ortn_cmd.z() = req.orientation[2]; - ortn_cmd.w() = req.orientation[3]; - - wam.moveTo(ortn_cmd, false); - return true; - } - -//Function to open the BarrettHand Grasp -template - bool WamNode::handOpenGrasp(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res) - { - ROS_INFO("Opening the BarrettHand Grasp"); - hand->open(Hand::GRASP, false); - return true; - } - -//Function to close the BarrettHand Grasp -template - bool WamNode::handCloseGrasp(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res) - { - ROS_INFO("Closing the BarrettHand Grasp"); - hand->close(Hand::GRASP, false); - return true; - } - -//Function to open the BarrettHand Spread -template - bool WamNode::handOpenSpread(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res) - { - ROS_INFO("Opening the BarrettHand Spread"); - hand->open(Hand::SPREAD, false); - return true; - } - -//Function to close the BarrettHand Spread -template - bool WamNode::handCloseSpread(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res) - { - ROS_INFO("Closing the BarrettHand Spread"); - hand->close(Hand::SPREAD, false); - return true; - } - -//Function to control a BarrettHand Finger Position -template - bool WamNode::handFingerPos(wam_srvs::BHandFingerPos::Request &req, wam_srvs::BHandFingerPos::Response &res) - { - ROS_INFO("Moving BarrettHand to Finger Positions: %.3f, %.3f, %.3f radians", req.radians[0], req.radians[1], - req.radians[2]); - hand->trapezoidalMove(Hand::jp_type(req.radians[0], req.radians[1], req.radians[2], 0.0), Hand::GRASP, false); - return true; - } - -//Function to control the BarrettHand Grasp Position -template - bool WamNode::handGraspPos(wam_srvs::BHandGraspPos::Request &req, wam_srvs::BHandGraspPos::Response &res) - { - ROS_INFO("Moving BarrettHand Grasp: %.3f radians", req.radians); - hand->trapezoidalMove(Hand::jp_type(req.radians), Hand::GRASP, false); - return true; - } - -//Function to control the BarrettHand Spread Position -template - bool WamNode::handSpreadPos(wam_srvs::BHandSpreadPos::Request &req, wam_srvs::BHandSpreadPos::Response &res) - { - ROS_INFO("Moving BarrettHand Spread: %.3f radians", req.radians); - hand->trapezoidalMove(Hand::jp_type(req.radians), Hand::SPREAD, false); - return true; - } - -//Function to control a BarrettHand Finger Velocity -template - bool WamNode::handFingerVel(wam_srvs::BHandFingerVel::Request &req, wam_srvs::BHandFingerVel::Response &res) - { - ROS_INFO("Moving BarrettHand Finger Velocities: %.3f, %.3f, %.3f m/s", req.velocity[0], req.velocity[1], - req.velocity[2]); - hand->velocityMove(Hand::jv_type(req.velocity[0], req.velocity[1], req.velocity[2], 0.0), Hand::GRASP); - return true; - } - -//Function to control a BarrettHand Grasp Velocity -template - bool WamNode::handGraspVel(wam_srvs::BHandGraspVel::Request &req, wam_srvs::BHandGraspVel::Response &res) - { - ROS_INFO("Moving BarrettHand Grasp: %.3f m/s", req.velocity); - hand->velocityMove(Hand::jv_type(req.velocity), Hand::GRASP); - return true; - } - -//Function to control a BarrettHand Spread Velocity -template - bool WamNode::handSpreadVel(wam_srvs::BHandSpreadVel::Request &req, wam_srvs::BHandSpreadVel::Response &res) - { - ROS_INFO("Moving BarrettHand Spread: %.3f m/s", req.velocity); - usleep(5000); - hand->velocityMove(Hand::jv_type(req.velocity), Hand::SPREAD); - return true; - } - -//Callback function for RT Cartesian Velocity messages -template - void WamNode::cartVelCB(const wam_msgs::RTCartVel::ConstPtr& msg) - { - if (cart_vel_status) - { - for (size_t i = 0; i < 3; i++) - rt_cv_cmd[i] = msg->direction[i]; - new_rt_cmd = true; - if (msg->magnitude != 0) - cart_vel_mag = msg->magnitude; - } - last_cart_vel_msg_time = ros::Time::now(); - - } - -//Callback function for RT Orientation Velocity Messages -template - void WamNode::ortnVelCB(const wam_msgs::RTOrtnVel::ConstPtr& msg) - { - if (ortn_vel_status) - { - for (size_t i = 0; i < 3; i++) - rt_ortn_cmd[i] = msg->angular[i]; - new_rt_cmd = true; - if (msg->magnitude != 0) - ortn_vel_mag = msg->magnitude; - } - last_ortn_vel_msg_time = ros::Time::now(); - } - -//Callback function for RT Joint Velocity Messages -template - void WamNode::jntVelCB(const wam_msgs::RTJointVel::ConstPtr& msg) - { - if (msg->velocities.size() != DOF) - { - ROS_INFO("Commanded Joint Velocities != DOF of WAM"); - return; - } - if (jnt_vel_status) - { - for (size_t i = 0; i < DOF; i++) - rt_jv_cmd[i] = msg->velocities[i]; - new_rt_cmd = true; - } - last_jnt_vel_msg_time = ros::Time::now(); - } - -//Callback function for RT Joint Position Messages -template - void WamNode::jntPosCB(const wam_msgs::RTJointPos::ConstPtr& msg) - { - if (msg->joints.size() != DOF) - { - ROS_INFO("Commanded Joint Positions != DOF of WAM"); - return; - } - if (jnt_pos_status) - { - for (size_t i = 0; i < DOF; i++) - { - rt_jp_cmd[i] = msg->joints[i]; - rt_jp_rl[i] = msg->rate_limits[i]; - } - new_rt_cmd = true; - } - last_jnt_pos_msg_time = ros::Time::now(); - } - -//Callback function for RT Cartesian Position Messages -template - void WamNode::cartPosCB(const wam_msgs::RTCartPos::ConstPtr& msg) - { - if (cart_pos_status) - { - for (size_t i = 0; i < 3; i++) - { - rt_cp_cmd[i] = msg->position[i]; - rt_cp_rl[i] = msg->rate_limits[i]; - } - new_rt_cmd = true; - } - last_cart_pos_msg_time = ros::Time::now(); - } - -//Function to update the WAM publisher -template - void WamNode::publishWam(ProductManager& pm) - { - //Current values to be published - jp_type jp = wam.getJointPositions(); - jt_type jt = wam.getJointTorques(); - jv_type jv = wam.getJointVelocities(); - cp_type cp_pub = wam.getToolPosition(); - Eigen::Quaterniond to_pub = wam.getToolOrientation(); - - //publishing sensor_msgs/JointState to wam/joint_states - for (size_t i = 0; i < DOF; i++) - { - wam_joint_state.position[i] = jp[i]; - wam_joint_state.velocity[i] = jv[i]; - wam_joint_state.effort[i] = jt[i]; - } - wam_joint_state.header.stamp = ros::Time::now(); - wam_joint_state_pub.publish(wam_joint_state); - - //publishing geometry_msgs/PoseStamed to wam/pose - wam_pose.header.stamp = ros::Time::now(); - wam_pose.pose.position.x = cp_pub[0]; - wam_pose.pose.position.y = cp_pub[1]; - wam_pose.pose.position.z = cp_pub[2]; - wam_pose.pose.orientation.w = to_pub.w(); - wam_pose.pose.orientation.x = to_pub.x(); - wam_pose.pose.orientation.y = to_pub.y(); - wam_pose.pose.orientation.z = to_pub.z(); - wam_pose_pub.publish(wam_pose); - } - -//Function to update the real-time control loops -template - void WamNode::publishHand() //systems::PeriodicDataLogger& logger - { - while (ros::ok()) - { - hand->update(); // Update the hand sensors - Hand::jp_type hi = hand->getInnerLinkPosition(); // get finger positions information - Hand::jp_type ho = hand->getOuterLinkPosition(); - for (size_t i = 0; i < 4; i++) // Save finger positions - bhand_joint_state.position[i] = hi[i]; - for (size_t j = 0; j < 3; j++) - bhand_joint_state.position[j + 4] = ho[j]; - bhand_joint_state.header.stamp = ros::Time::now(); // Set the timestamp - bhand_joint_state_pub.publish(bhand_joint_state); // Publish the BarrettHand joint states - btsleep(1.0 / BHAND_PUBLISH_FREQ); // Sleep according to the specified publishing frequency - } - } - -//Function to update the real-time control loops -template - void WamNode::updateRT(ProductManager& pm) //systems::PeriodicDataLogger& logger - { - //Real-Time Cartesian Velocity Control Portion - if (last_cart_vel_msg_time + rt_msg_timeout > ros::Time::now()) // checking if a cartesian velocity message has been published and if it is within timeout - { - if (!cart_vel_status) - { - cart_dir.setValue(cp_type(0.0, 0.0, 0.0)); // zeroing the cartesian direction - current_cart_pos.setValue(wam.getToolPosition()); // Initializing the cartesian position - current_ortn.setValue(wam.getToolOrientation()); // Initializing the orientation - systems::forceConnect(ramp.output, mult_linear.input1); // connecting the ramp to multiplier - systems::forceConnect(cart_dir.output, mult_linear.input2); // connecting the direction to the multiplier - systems::forceConnect(mult_linear.output, cart_pos_sum.getInput(0)); // adding the output of the multiplier - systems::forceConnect(current_cart_pos.output, cart_pos_sum.getInput(1)); // with the starting cartesian position offset - systems::forceConnect(cart_pos_sum.output, rt_pose_cmd.getInput<0>()); // saving summed position as new commanded pose.position - systems::forceConnect(current_ortn.output, rt_pose_cmd.getInput<1>()); // saving the original orientation to the pose.orientation - ramp.setSlope(cart_vel_mag); // setting the slope to the commanded magnitude - ramp.stop(); // ramp is stopped on startup - ramp.setOutput(0.0); // ramp is re-zeroed on startup - ramp.start(); // start the ramp - wam.trackReferenceSignal(rt_pose_cmd.output); // command WAM to track the RT commanded (500 Hz) updated pose - } - else if (new_rt_cmd) - { - ramp.reset(); // reset the ramp to 0 - ramp.setSlope(cart_vel_mag); - cart_dir.setValue(rt_cv_cmd); // set our cartesian direction to subscribed command - current_cart_pos.setValue(wam.tpoTpController.referenceInput.getValue()); // updating the current position to the actual low level commanded value - } - cart_vel_status = true; - new_rt_cmd = false; - } - - //Real-Time Angular Velocity Control Portion - else if (last_ortn_vel_msg_time + rt_msg_timeout > ros::Time::now()) // checking if a orientation velocity message has been published and if it is within timeout - { - if (!ortn_vel_status) - { - rpy_cmd.setValue(math::Vector<3>::type(0.0, 0.0, 0.0)); // zeroing the rpy command - current_cart_pos.setValue(wam.getToolPosition()); // Initializing the cartesian position - current_rpy_ortn.setValue(toRPY(wam.getToolOrientation())); // Initializing the orientation - - systems::forceConnect(ramp.output, mult_angular.input1); // connecting the ramp to multiplier - systems::forceConnect(rpy_cmd.output, mult_angular.input2); // connecting the rpy command to the multiplier - systems::forceConnect(mult_angular.output, ortn_cmd_sum.getInput(0)); // adding the output of the multiplier - systems::forceConnect(current_rpy_ortn.output, ortn_cmd_sum.getInput(1)); // with the starting rpy orientation offset - systems::forceConnect(ortn_cmd_sum.output, to_quat.input); - systems::forceConnect(current_cart_pos.output, rt_pose_cmd.getInput<0>()); // saving the original position to the pose.position - systems::forceConnect(to_quat.output, rt_pose_cmd.getInput<1>()); // saving the summed and converted new quaternion commmand as the pose.orientation - ramp.setSlope(ortn_vel_mag); // setting the slope to the commanded magnitude - ramp.stop(); // ramp is stopped on startup - ramp.setOutput(0.0); // ramp is re-zeroed on startup - ramp.start(); // start the ramp - wam.trackReferenceSignal(rt_pose_cmd.output); // command the WAM to track the RT commanded up to (500 Hz) cartesian velocity - } - else if (new_rt_cmd) - { - ramp.reset(); // reset the ramp to 0 - ramp.setSlope(ortn_vel_mag); // updating the commanded angular velocity magnitude - rpy_cmd.setValue(rt_ortn_cmd); // set our angular rpy command to subscribed command - current_rpy_ortn.setValue(toRPY(wam.tpoToController.referenceInput.getValue())); // updating the current orientation to the actual low level commanded value - } - ortn_vel_status = true; - new_rt_cmd = false; - } - - //Real-Time Joint Velocity Control Portion - else if (last_jnt_vel_msg_time + rt_msg_timeout > ros::Time::now()) // checking if a joint velocity message has been published and if it is within timeout - { - if (!jnt_vel_status) - { - jv_type jv_start; - for (size_t i = 0; i < DOF; i++) - jv_start[i] = 0.0; - jv_track.setValue(jv_start); // zeroing the joint velocity command - wam.trackReferenceSignal(jv_track.output); // command the WAM to track the RT commanded up to (500 Hz) joint velocities - } - else if (new_rt_cmd) - { - jv_track.setValue(rt_jv_cmd); // set our joint velocity to subscribed command - } - jnt_vel_status = true; - new_rt_cmd = false; - } - - //Real-Time Joint Position Control Portion - else if (last_jnt_pos_msg_time + rt_msg_timeout > ros::Time::now()) // checking if a joint position message has been published and if it is within timeout - { - if (!jnt_pos_status) - { - jp_type jp_start = wam.getJointPositions(); - jp_track.setValue(jp_start); // setting initial the joint position command - jp_rl.setLimit(rt_jp_rl); - systems::forceConnect(jp_track.output, jp_rl.input); - wam.trackReferenceSignal(jp_rl.output); // command the WAM to track the RT commanded up to (500 Hz) joint positions - } - else if (new_rt_cmd) - { - jp_track.setValue(rt_jp_cmd); // set our joint position to subscribed command - jp_rl.setLimit(rt_jp_rl); // set our rate limit to subscribed rate to control the rate of the moves - } - jnt_pos_status = true; - new_rt_cmd = false; - } - - //Real-Time Cartesian Position Control Portion - else if (last_cart_pos_msg_time + rt_msg_timeout > ros::Time::now()) // checking if a cartesian position message has been published and if it is within timeout - { - if (!cart_pos_status) - { - cp_track.setValue(wam.getToolPosition()); - current_ortn.setValue(wam.getToolOrientation()); // Initializing the orientation - cp_rl.setLimit(rt_cp_rl); - systems::forceConnect(cp_track.output, cp_rl.input); - systems::forceConnect(cp_rl.output, rt_pose_cmd.getInput<0>()); // saving the rate limited cartesian position command to the pose.position - systems::forceConnect(current_ortn.output, rt_pose_cmd.getInput<1>()); // saving the original orientation to the pose.orientation - wam.trackReferenceSignal(rt_pose_cmd.output); //Commanding the WAM to track the real-time pose command. - } - else if (new_rt_cmd) - { - cp_track.setValue(rt_cp_cmd); // Set our cartesian positions to subscribed command - cp_rl.setLimit(rt_cp_rl); // Updating the rate limit to subscribed rate to control the rate of the moves - } - cart_pos_status = true; - new_rt_cmd = false; - } - - //If we fall out of 'Real-Time', hold joint positions - else if (cart_vel_status | ortn_vel_status | jnt_vel_status | jnt_pos_status | cart_pos_status) - { - wam.moveTo(wam.getJointPositions()); // Holds current joint positions upon a RT message timeout - cart_vel_status = ortn_vel_status = jnt_vel_status = jnt_pos_status = cart_pos_status = ortn_pos_status = false; - } - } - -//wam_main Function -template - int wam_main(int argc, char** argv, ProductManager& pm, systems::Wam& wam) - { - BARRETT_UNITS_TEMPLATE_TYPEDEFS(DOF); - ros::init(argc, argv, "wam_node"); - WamNode wam_node(wam); - wam_node.init(pm); - ros::Rate pub_rate(PUBLISH_FREQ); - - if (pm.getHand()) - boost::thread handPubThread(&WamNode::publishHand, &wam_node); - - while (ros::ok() && pm.getSafetyModule()->getMode() == SafetyModule::ACTIVE) - { - ros::spinOnce(); - wam_node.publishWam(pm); - wam_node.updateRT(pm); - pub_rate.sleep(); - } - - return 0; - } diff --git a/wam_node_sim/src/wam_node_sim.cpp b/wam_node_sim/src/wam_node_sim.cpp index 660e713..10fcf0e 100644 --- a/wam_node_sim/src/wam_node_sim.cpp +++ b/wam_node_sim/src/wam_node_sim.cpp @@ -35,10 +35,7 @@ #include - -#include // BarrettHand threading -#include - +#include // BarrettHand threading #include @@ -820,7 +817,7 @@ int main(int argc,char *argv[]) WamNode<7> wam_node(wam,bhand); ros::Rate loop(PUBLISH_FREQ); - if(useHand) boost::thread handPubThread(&WamNode<7>::publishHand,&wam_node); + if(useHand) std::thread handPubThread(&WamNode<7>::publishHand,&wam_node); while (ros::ok()) {