From: Walter Fetter Lages Date: Thu, 27 Jun 2019 06:00:59 +0000 (-0300) Subject: Add wam_teleoplib. X-Git-Url: http://git.ece.ufrgs.br/?a=commitdiff_plain;h=a14ec57efa93d5ca1a4e59ba0004a7689002cb29;p=ufrgs_wam.git Add wam_teleoplib. --- diff --git a/wam_node_test/CMakeLists.txt b/wam_node_test/CMakeLists.txt index 82461c7..43f5c4c 100644 --- a/wam_node_test/CMakeLists.txt +++ b/wam_node_test/CMakeLists.txt @@ -9,7 +9,7 @@ add_compile_options(-std=c++11) ## is used, also find other catkin packages find_package(catkin REQUIRED COMPONENTS roscpp - wam_node_sim + wam_teleoplib ) ## System dependencies are found with CMake's conventions @@ -104,7 +104,7 @@ find_package(catkin REQUIRED COMPONENTS catkin_package( # INCLUDE_DIRS include # LIBRARIES wam_node_test -# CATKIN_DEPENDS roscpp wam_node_sim +# CATKIN_DEPENDS roscpp wam_teleoplib # DEPENDS system_lib ) diff --git a/wam_node_test/package.xml b/wam_node_test/package.xml index 4db3482..6fc9bea 100644 --- a/wam_node_test/package.xml +++ b/wam_node_test/package.xml @@ -51,11 +51,11 @@ catkin roscpp - wam_node_sim + wam_teleoplib roscpp - wam_node_sim + wam_teleoplib roscpp - wam_node_sim + wam_teleoplib diff --git a/wam_node_test/src/wam_node_test.cpp b/wam_node_test/src/wam_node_test.cpp index 5050f19..6df3900 100644 --- a/wam_node_test/src/wam_node_test.cpp +++ b/wam_node_test/src/wam_node_test.cpp @@ -18,450 +18,16 @@ *******************************************************************************/ -#include - -#include -#include - -#include -#include - -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include - -class WamNodeOp -{ - public: - static const size_t DOF=7; - typedef Eigen::Matrix jt_type; - typedef Eigen::Matrix jp_type; - typedef Eigen::Matrix jv_type; - typedef Eigen::Matrix cp_type; - typedef Eigen::Matrix cv_type; - typedef std::tuple pose_type; - - WamNodeOp(const ros::NodeHandle &node,double updateRate,double jointTolerance,double jointRateLimit,double cartTolerance,double cartRateLimit); - ~WamNodeOp(void); - void jointPos(const jp_type &jpCmd,bool block=true); - void jointWait(const jp_type &jpCmd) const; - void fingerPos(double f1,double f2,double f3,bool block=true); - void graspPos(double pos,bool block=true); - void fingerWait(double f1,double f2,double f3) const; - void spreadPos(double spread,bool block=true); - void spreadWait(double spread) const; - void closeGrasp(bool block=true); - void closeSpread(bool block=true); - void openGrasp(bool block=true); - void openSpread(bool block=true); - void goHome(void); - void holdCartPos(bool hold); - void holdJointPos(bool hold); - void holdOrtnPos(bool hold); - void jointMove(const jp_type &jpCmd,bool block=true); - void trackJointPos(bool track); - void cartPosMove(const cp_type &cpCmd,bool block=true); - void cartPosWait(const cp_type &cpCmd) const; - void ortnMove(const Eigen::Quaterniond &ortnCmd,bool block=true); - void ortnWait(const Eigen::Quaterniond &ortnCmd) const; - void poseMove(const pose_type &poseCmd,bool block=true); - void cartPos(const cp_type &cpCmd,bool block=true); - void trackCartPos(bool track); - - private: - void jointStatesCB(const sensor_msgs::JointState &jointStates); - void poseCB(const geometry_msgs::PoseStamped &pose); - void update(void) const; - - ros::NodeHandle node_; - ros::Publisher jointPosPub_; - ros::Publisher cartPosPub_; - ros::Subscriber jointStatesSub_; - ros::Subscriber poseSub_; - ros::ServiceClient fingerPosCli_; - ros::ServiceClient spreadPosCli_; - ros::ServiceClient graspPosCli_; - ros::ServiceClient closeGraspCli_; - ros::ServiceClient closeSpreadCli_; - ros::ServiceClient openGraspCli_; - ros::ServiceClient openSpreadCli_; - ros::ServiceClient goHomeCli_; - ros::ServiceClient holdCartPosCli_; - ros::ServiceClient holdJointPosCli_; - ros::ServiceClient holdOrtnCli_; - ros::ServiceClient jointMoveCli_; - ros::ServiceClient cartPosMoveCli_; - ros::ServiceClient ortnMoveCli_; - ros::ServiceClient poseMoveCli_; - - jp_type jpCmd_; - cp_type cpCmd_; - jp_type wamJp_; - cp_type wamCp_; - Eigen::Quaterniond wamOrtn_; - Eigen::Vector4d bhandJp_; - - std::thread update_; - bool runUpdate_; - bool trackJointPos_; - bool trackCartPos_; - - double updateRate_; - double jointTolerance_; - double jointRateLimit_; - double cartTolerance_; - double cartRateLimit_; - const double msgTimeout; - - public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW -}; - -WamNodeOp::WamNodeOp(const ros::NodeHandle &node,double updateRate,double jointTolerance,double jointRateLimit,double cartTolerance,double cartRateLimit): - runUpdate_(true),trackJointPos_(false),trackCartPos_(false),msgTimeout(0.4) -{ - node_=node; - jointPosPub_=node_.advertise("wam/jnt_pos_cmd",10); - cartPosPub_=node_.advertise("wam/cart_pos_cmd",10); - closeGraspCli_=node_.serviceClient("bhand/close_grasp"); - closeSpreadCli_=node_.serviceClient("bhand/close_spread"); - openGraspCli_=node_.serviceClient("bhand/open_grasp"); - openSpreadCli_=node_.serviceClient("bhand/open_spread"); - fingerPosCli_=node_.serviceClient("bhand/finger_pos"); - spreadPosCli_=node_.serviceClient("bhand/spread_pos"); - graspPosCli_=node_.serviceClient("bhand/grasp_pos"); - cartPosMoveCli_=node_.serviceClient("wam/cart_move"); - goHomeCli_=node_.serviceClient("wam/go_home"); - holdCartPosCli_=node_.serviceClient("wam/hold_cart_pos"); - holdJointPosCli_=node_.serviceClient("wam/hold_joint_pos"); - holdOrtnCli_=node_.serviceClient("wam/hold_ortn"); - jointMoveCli_=node_.serviceClient("wam/joint_move"); - ortnMoveCli_=node_.serviceClient("wam/ortn_move"); - poseMoveCli_=node_.serviceClient("wam/pose_move"); -// gravityCompCli_=node_.serviceClient("wam/gravity_comp"); - jointStatesSub_=node_.subscribe("/joint_states",10,&WamNodeOp::jointStatesCB,this); - poseSub_=node_.subscribe("wam/pose",10,&WamNodeOp::poseCB,this); - - jointTolerance_=jointTolerance; - updateRate_=updateRate; - jointRateLimit_=jointRateLimit; - cartTolerance_=cartTolerance; - cartRateLimit_=cartRateLimit; - - update_=std::thread(&WamNodeOp::update,this); -} - -WamNodeOp::~WamNodeOp(void) -{ - runUpdate_=false; - update_.join(); - jointPosPub_.shutdown(); - cartPosPub_.shutdown(); - jointStatesSub_.shutdown(); - poseSub_.shutdown(); -} - -void WamNodeOp::jointPos(const jp_type &jpCmd,bool block) -{ - jpCmd_=jpCmd; - trackJointPos(true); - if(block) jointWait(jpCmd); -} - -void WamNodeOp::trackJointPos(bool track) -{ - trackJointPos_=track; - ros::Duration(msgTimeout).sleep(); -} - -void WamNodeOp::jointWait(const jp_type &jpCmd) const -{ - while((wamJp_-jpCmd).norm() > wamJp_.size()*jointTolerance_) - ros::Duration(0.01).sleep(); -} - -void WamNodeOp::fingerPos(double f1,double f2,double f3,bool block) -{ - wam_srvs::BHandFingerPos fingerPosSrv; - - fingerPosSrv.request.radians[0]=f1; - fingerPosSrv.request.radians[1]=f2; - fingerPosSrv.request.radians[2]=f3; - if(fingerPosCli_.call(fingerPosSrv)) ROS_INFO_STREAM("Finger moved to " << f1 << " " << f2 << " " << f3 << "."); - else ROS_ERROR_STREAM("Error moving finger."); - if(block) fingerWait(f1,f2,f3); -} - -void WamNodeOp::fingerWait(double f1,double f2,double f3) const -{ - Eigen::Vector3d jp(f1,f2,f3); - - while((jp-bhandJp_.head(3)).norm() > 3*jointTolerance_) - ros::Duration(0.01).sleep(); -} - -void WamNodeOp::graspPos(double pos,bool block) -{ - wam_srvs::BHandGraspPos graspPosSrv; - - graspPosSrv.request.radians=pos; - if(graspPosCli_.call(graspPosSrv)) ROS_INFO_STREAM("Grasp moved to " << pos << "."); - else ROS_ERROR_STREAM("Error moving grasp."); - if(block) fingerWait(pos,pos,pos); -} - -void WamNodeOp::spreadPos(double spread,bool block) -{ - wam_srvs::BHandSpreadPos spreadPosSrv; - - spreadPosSrv.request.radians=spread; - if(spreadPosCli_.call(spreadPosSrv)) ROS_INFO_STREAM("Spread moved to " << spread << "."); - else ROS_ERROR_STREAM("Error moving spread."); - if(block) spreadWait(spread); -} - -void WamNodeOp::spreadWait(double spread) const -{ - while(fabs(spread-bhandJp_[3]) > jointTolerance_) - ros::Duration(0.01).sleep(); -} - -void WamNodeOp::closeGrasp(bool block) -{ - std_srvs::Empty emptySrv; - - if(closeGraspCli_.call(emptySrv)) ROS_INFO_STREAM("Grasp closed."); - else ROS_ERROR_STREAM("Error closing grasp."); - if(block) fingerWait(2.44,2.44,2.44); -} - -void WamNodeOp::closeSpread(bool block) -{ - std_srvs::Empty emptySrv; - - if(closeSpreadCli_.call(emptySrv)) ROS_INFO_STREAM("Spread closed."); - else ROS_ERROR_STREAM("Error closing spread."); - if(block) spreadWait(M_PI); -} - -void WamNodeOp::openGrasp(bool block) -{ - std_srvs::Empty emptySrv; - - if(openGraspCli_.call(emptySrv)) ROS_INFO_STREAM("Grasp open."); - else ROS_ERROR_STREAM("Error opening grasp."); - if(block) fingerWait(0,0,0); -} - -void WamNodeOp::openSpread(bool block) -{ - std_srvs::Empty emptySrv; - - if(openSpreadCli_.call(emptySrv)) ROS_INFO_STREAM("Spread open."); - else ROS_ERROR_STREAM("Error opening spread."); - if(block) spreadWait(0.0); -} - -void WamNodeOp::goHome(void) -{ - std_srvs::Empty emptySrv; - - if(goHomeCli_.call(emptySrv)) ROS_INFO_STREAM("Returned home position."); - else ROS_ERROR_STREAM("Error returning home position."); -} - -void WamNodeOp::holdCartPos(bool hold) -{ - wam_srvs::Hold holdSrv; - - holdSrv.request.hold=hold; - if(holdCartPosCli_.call(holdSrv)) ROS_INFO_STREAM("Cartesian position held."); - else ROS_ERROR_STREAM("Error holding Cartesian position."); -} - -void WamNodeOp::holdJointPos(bool hold) -{ - wam_srvs::Hold holdSrv; - - holdSrv.request.hold=hold; - if(holdJointPosCli_.call(holdSrv)) ROS_INFO_STREAM("Joint position held."); - else ROS_ERROR_STREAM("Error holding joint position."); -} - -void WamNodeOp::holdOrtnPos(bool hold) -{ - wam_srvs::Hold holdSrv; - - holdSrv.request.hold=hold; - if(holdOrtnCli_.call(holdSrv)) ROS_INFO_STREAM("Orientation held."); - else ROS_ERROR_STREAM("Error holding orientation."); -} - -void WamNodeOp::jointMove(const jp_type &jpCmd,bool block) -{ - wam_srvs::JointMove jointMoveSrv; - - for(int i=0;i < jpCmd.size();i++) jointMoveSrv.request.joints.push_back(jpCmd[i]); - if(jointMoveCli_.call(jointMoveSrv)) - ROS_INFO_STREAM("Joints moved to " << jpCmd.transpose() << "."); - else ROS_ERROR_STREAM("Error moving joints."); - if(block) jointWait(jpCmd); -} - -void WamNodeOp::cartPosMove(const cp_type &cpCmd,bool block) -{ - wam_srvs::CartPosMove cartPosMoveSrv; - - for(int i=0;i < 3;i++) cartPosMoveSrv.request.position[i]=cpCmd[i]; - if(cartPosMoveCli_.call(cartPosMoveSrv)) - ROS_INFO_STREAM("Cartesian posiiton moved to " << cpCmd.transpose() << "."); - else ROS_ERROR_STREAM("Error moving to Cartesian position."); - if(block) cartPosWait(cpCmd); -} - -void WamNodeOp::cartPosWait(const cp_type &cpCmd) const -{ - while((wamCp_-cpCmd).norm() > cartTolerance_) - ros::Duration(0.01).sleep(); -} - -void WamNodeOp::ortnMove(const Eigen::Quaterniond &ortnCmd,bool block) -{ - wam_srvs::OrtnMove ortnMoveSrv; - - ortnMoveSrv.request.orientation[0]=ortnCmd.x(); - ortnMoveSrv.request.orientation[1]=ortnCmd.y(); - ortnMoveSrv.request.orientation[2]=ortnCmd.z(); - ortnMoveSrv.request.orientation[3]=ortnCmd.w(); - if(ortnMoveCli_.call(ortnMoveSrv)) ROS_INFO_STREAM("Tool orientation moved to " - << ortnCmd.x() << " " << ortnCmd.y() << " " << ortnCmd.z() << " " << ortnCmd.w() << "."); - else ROS_ERROR_STREAM("Error moving tool orientation."); - if(block) ortnWait(ortnCmd); -} - -void WamNodeOp::ortnWait(const Eigen::Quaterniond &ortnCmd) const -{ - while(fabs(ortnCmd.angularDistance(wamOrtn_)) > 8*jointTolerance_); - ros::Duration(0.01).sleep(); -} - -void WamNodeOp::poseMove(const pose_type &poseCmd,bool block) -{ - wam_srvs::PoseMove poseMoveSrv; - - tf::pointEigenToMsg(std::get<0>(poseCmd),poseMoveSrv.request.pose.position); - tf::quaternionEigenToMsg(std::get<1>(poseCmd),poseMoveSrv.request.pose.orientation); - - if(poseMoveCli_.call(poseMoveSrv)) - ROS_INFO_STREAM("Pose moved to " << std::get<0>(poseCmd).transpose() << " " - << std::get<1>(poseCmd).x() << " " << std::get<1>(poseCmd).y() << " " << std::get<1>(poseCmd).z() << " " << std::get<1>(poseCmd).w() << "."); - else ROS_ERROR_STREAM("Error moving to pose."); - if(block) - { - cartPosWait(std::get<0>(poseCmd)); - ortnWait(std::get<1>(poseCmd)); - } -} - -void WamNodeOp::cartPos(const cp_type &cpCmd,bool block) -{ - for(int i=0;i < cpCmd_.size();i++) cpCmd_[i]=cpCmd[i]; - - trackCartPos(true); - - if(block) cartPosWait(cpCmd); -} - -void WamNodeOp::trackCartPos(bool track) -{ - trackCartPos_=track; - ros::Duration(msgTimeout).sleep(); -} - -void WamNodeOp::update(void) const -{ - ros::Rate loop(updateRate_); - while(ros::ok() && runUpdate_) - { - if(trackJointPos_) - { - wam_msgs::RTJointPos jointPosMsg; - - for(int i=0;i < jpCmd_.size();i++) jointPosMsg.joints.push_back(jpCmd_[i]); - jointPosMsg.rate_limits.resize(jpCmd_.size(),jointRateLimit_); - jointPosPub_.publish(jointPosMsg); - } - - if(trackCartPos_) - { - wam_msgs::RTCartPos cartPosMsg; - - for(int i=0;i < cpCmd_.size();i++) - { - cartPosMsg.position[i]=cpCmd_[i]; - cartPosMsg.rate_limits[i]=cartRateLimit_; - } - cartPosPub_.publish(cartPosMsg); - } - - ros::spinOnce(); - loop.sleep(); - } -} - -void WamNodeOp::jointStatesCB(const sensor_msgs::JointState &jointStates) -{ - std::vector jointNames; - jointNames.push_back("wam_joint_1"); - jointNames.push_back("wam_joint_2"); - jointNames.push_back("wam_joint_3"); - jointNames.push_back("wam_joint_4"); - jointNames.push_back("wam_joint_5"); - jointNames.push_back("wam_joint_6"); - jointNames.push_back("wam_joint_7"); - jointNames.push_back("bhand_finger1_joint_2"); - jointNames.push_back("bhand_finger2_joint_2"); - jointNames.push_back("bhand_finger3_joint_2"); - jointNames.push_back("bhand_spread"); - - for(int i=0;i < 7;i++) - { - int j=find(jointStates.name.begin(),jointStates.name.end(),jointNames[i]) - jointStates.name.begin(); - wamJp_[i]=jointStates.position[j]; - } - - for(int i=0;i < 4;i++) - { - int j=find(jointStates.name.begin(),jointStates.name.end(),jointNames[i+7]) - jointStates.name.begin(); - bhandJp_[i]=jointStates.position[j]; - } -} - -void WamNodeOp::poseCB(const geometry_msgs::PoseStamped &pose) -{ - tf::pointMsgToEigen(pose.pose.position,wamCp_); - tf::quaternionMsgToEigen(pose.pose.orientation,wamOrtn_); -} +#include int main(int argc,char* argv[]) { ros::init(argc,argv,"wam_node_test"); ros::NodeHandle node; - WamNodeOp wam(node,50.0,0.02,0.5,0.005,0.1); + wam::WamTeleop wam(node,50.0,0.02,0.5,0.005,0.1); - WamNodeOp::jp_type jpCmd; + wam::WamTeleop::jp_type jpCmd; jpCmd.setZero(); jpCmd[1]=-2.0; jpCmd[3]=3.1-0.4; @@ -500,7 +66,7 @@ int main(int argc,char* argv[]) wam.goHome(); - WamNodeOp::cp_type cpCmd; + wam::WamTeleop::cp_type cpCmd; cpCmd[0]=0.0; cpCmd[1]=-0.6; cpCmd[2]=1.3; diff --git a/wam_teleoplib/CMakeLists.txt b/wam_teleoplib/CMakeLists.txt new file mode 100644 index 0000000..bfda9ea --- /dev/null +++ b/wam_teleoplib/CMakeLists.txt @@ -0,0 +1,204 @@ +cmake_minimum_required(VERSION 2.8.3) +project(wam_teleoplib) + +## Compile as C++11, supported in ROS Kinetic and newer +add_compile_options(-std=c++11) + +## Find catkin macros and libraries +## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) +## is used, also find other catkin packages +find_package(catkin REQUIRED COMPONENTS + roscpp + wam_msgs + wam_srvs + eigen_conversions +) + +## System dependencies are found with CMake's conventions +# find_package(Boost REQUIRED COMPONENTS system) + +find_package(Eigen3 REQUIRED) + +## Uncomment this if the package has a setup.py. This macro ensures +## modules and global scripts declared therein get installed +## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html +# catkin_python_setup() + +################################################ +## Declare ROS messages, services and actions ## +################################################ + +## To declare and build messages, services or actions from within this +## package, follow these steps: +## * Let MSG_DEP_SET be the set of packages whose message types you use in +## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). +## * In the file package.xml: +## * add a build_depend tag for "message_generation" +## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET +## * If MSG_DEP_SET isn't empty the following dependency has been pulled in +## but can be declared for certainty nonetheless: +## * add a exec_depend tag for "message_runtime" +## * In this file (CMakeLists.txt): +## * add "message_generation" and every package in MSG_DEP_SET to +## find_package(catkin REQUIRED COMPONENTS ...) +## * add "message_runtime" and every package in MSG_DEP_SET to +## catkin_package(CATKIN_DEPENDS ...) +## * uncomment the add_*_files sections below as needed +## and list every .msg/.srv/.action file to be processed +## * uncomment the generate_messages entry below +## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) + +## Generate messages in the 'msg' folder +# add_message_files( +# FILES +# Message1.msg +# Message2.msg +# ) + +## Generate services in the 'srv' folder +# add_service_files( +# FILES +# Service1.srv +# Service2.srv +# ) + +## Generate actions in the 'action' folder +# add_action_files( +# FILES +# Action1.action +# Action2.action +# ) + +## Generate added messages and services with any dependencies listed here +#generate_messages( +# DEPENDENCIES +# wam_msgs +# wam_srvs +#) + +################################################ +## Declare ROS dynamic reconfigure parameters ## +################################################ + +## To declare and build dynamic reconfigure parameters within this +## package, follow these steps: +## * In the file package.xml: +## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" +## * In this file (CMakeLists.txt): +## * add "dynamic_reconfigure" to +## find_package(catkin REQUIRED COMPONENTS ...) +## * uncomment the "generate_dynamic_reconfigure_options" section below +## and list every .cfg file to be processed + +## Generate dynamic reconfigure parameters in the 'cfg' folder +# generate_dynamic_reconfigure_options( +# cfg/DynReconf1.cfg +# cfg/DynReconf2.cfg +# ) + +################################### +## catkin specific configuration ## +################################### +## The catkin_package macro generates cmake config files for your package +## Declare things to be passed to dependent projects +## INCLUDE_DIRS: uncomment this if your package contains header files +## LIBRARIES: libraries you create in this project that dependent projects also need +## CATKIN_DEPENDS: catkin_packages dependent projects also need +## DEPENDS: system dependencies of this project that dependent projects also need +catkin_package( + INCLUDE_DIRS include + LIBRARIES wam_teleoplib +# CATKIN_DEPENDS roscpp wam_msgs wam_srvs + DEPENDS EIGEN3 +) + +########### +## Build ## +########### + +## Specify additional locations of header files +## Your package locations should be listed before other locations +include_directories( + include + ${catkin_INCLUDE_DIRS} + ${EIGEN3_INCLUDE_DIRS} +) + +## Declare a C++ library +add_library(${PROJECT_NAME} + src/wam_teleop.cpp +) + +## Add cmake target dependencies of the library +## as an example, code may need to be generated before libraries +## either from message generation or dynamic reconfigure +# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Declare a C++ executable +## With catkin_make all packages are built within a single CMake context +## The recommended prefix ensures that target names across packages don't collide +# add_executable(${PROJECT_NAME}_node src/wam_teleoplib_node.cpp) + +## Rename C++ executable without prefix +## The above recommended prefix causes long target names, the following renames the +## target back to the shorter version for ease of user use +## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" +# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") + +## Add cmake target dependencies of the executable +## same as for the library above +# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Specify libraries to link a library or executable target against +target_link_libraries(${PROJECT_NAME} + ${catkin_LIBRARIES} +# ${Eigen3_LIBRARIES} +) + +############# +## Install ## +############# + +# all install targets should use catkin DESTINATION variables +# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html + +## Mark executable scripts (Python etc.) for installation +## in contrast to setup.py, you can choose the destination +# install(PROGRAMS +# scripts/my_python_script +# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +## Mark executables and/or libraries for installation + install(TARGETS ${PROJECT_NAME} #${PROJECT_NAME}_node + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + +## Mark cpp header files for installation + install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} + FILES_MATCHING PATTERN "*.h" + PATTERN ".svn" EXCLUDE +) + +## Mark other files for installation (e.g. launch and bag files, etc.) +# install(FILES +# # myfile1 +# # myfile2 +# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +# ) + +############# +## Testing ## +############# + +## Add gtest based cpp test target and link libraries +# catkin_add_gtest(${PROJECT_NAME}-test test/test_wam_teleoplib.cpp) +# if(TARGET ${PROJECT_NAME}-test) +# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) +# endif() + +## Add folders to be run by python nosetests +# catkin_add_nosetests(test) diff --git a/wam_teleoplib/include/wam_teleoplib/wam_teleop.h b/wam_teleoplib/include/wam_teleoplib/wam_teleop.h new file mode 100644 index 0000000..226e38f --- /dev/null +++ b/wam_teleoplib/include/wam_teleoplib/wam_teleop.h @@ -0,0 +1,123 @@ +/****************************************************************************** + WAM Teleop Library + Copyright (C) 2019 Walter Fetter Lages + + This program 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 program 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 program. If not, see + . + +*******************************************************************************/ + +#ifndef WAM_TELEOP_H +#define WAM_TELEOP_H + +#include + +#include +#include + +#include + +#include +#include + +namespace wam +{ + class WamTeleop + { + public: + static const size_t DOF=7; + typedef Eigen::Matrix jt_type; + typedef Eigen::Matrix jp_type; + typedef Eigen::Matrix jv_type; + typedef Eigen::Matrix cp_type; + typedef Eigen::Matrix cv_type; + typedef std::tuple pose_type; + + WamTeleop(const ros::NodeHandle &node,double updateRate,double jointTolerance,double jointRateLimit,double cartTolerance,double cartRateLimit); + ~WamTeleop(void); + void jointPos(const jp_type &jpCmd,bool block=true); + void jointWait(const jp_type &jpCmd) const; + void fingerPos(double f1,double f2,double f3,bool block=true); + void graspPos(double pos,bool block=true); + void fingerWait(double f1,double f2,double f3) const; + void spreadPos(double spread,bool block=true); + void spreadWait(double spread) const; + void closeGrasp(bool block=true); + void closeSpread(bool block=true); + void openGrasp(bool block=true); + void openSpread(bool block=true); + void goHome(void); + void holdCartPos(bool hold); + void holdJointPos(bool hold); + void holdOrtnPos(bool hold); + void jointMove(const jp_type &jpCmd,bool block=true); + void trackJointPos(bool track); + void cartPosMove(const cp_type &cpCmd,bool block=true); + void cartPosWait(const cp_type &cpCmd) const; + void ortnMove(const Eigen::Quaterniond &ortnCmd,bool block=true); + void ortnWait(const Eigen::Quaterniond &ortnCmd) const; + void poseMove(const pose_type &poseCmd,bool block=true); + void cartPos(const cp_type &cpCmd,bool block=true); + void trackCartPos(bool track); + + private: + void jointStatesCB(const sensor_msgs::JointState &jointStates); + void poseCB(const geometry_msgs::PoseStamped &pose); + void update(void) const; + + ros::NodeHandle node_; + ros::Publisher jointPosPub_; + ros::Publisher cartPosPub_; + ros::Subscriber jointStatesSub_; + ros::Subscriber poseSub_; + ros::ServiceClient fingerPosCli_; + ros::ServiceClient spreadPosCli_; + ros::ServiceClient graspPosCli_; + ros::ServiceClient closeGraspCli_; + ros::ServiceClient closeSpreadCli_; + ros::ServiceClient openGraspCli_; + ros::ServiceClient openSpreadCli_; + ros::ServiceClient goHomeCli_; + ros::ServiceClient holdCartPosCli_; + ros::ServiceClient holdJointPosCli_; + ros::ServiceClient holdOrtnCli_; + ros::ServiceClient jointMoveCli_; + ros::ServiceClient cartPosMoveCli_; + ros::ServiceClient ortnMoveCli_; + ros::ServiceClient poseMoveCli_; + + jp_type jpCmd_; + cp_type cpCmd_; + jp_type wamJp_; + cp_type wamCp_; + Eigen::Quaterniond wamOrtn_; + Eigen::Vector4d bhandJp_; + + std::thread update_; + bool runUpdate_; + bool trackJointPos_; + bool trackCartPos_; + + double updateRate_; + double jointTolerance_; + double jointRateLimit_; + double cartTolerance_; + double cartRateLimit_; + const double msgTimeout; + + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + }; +} +#endif diff --git a/wam_teleoplib/package.xml b/wam_teleoplib/package.xml new file mode 100644 index 0000000..1b6508a --- /dev/null +++ b/wam_teleoplib/package.xml @@ -0,0 +1,75 @@ + + + wam_teleoplib + 0.0.0 + The wam_teleoplib package + + + + + Walter Fetter Lages + + + + + + GPLv3 + + + + + + + + + + + + + Walter Fetter Lages + + + + + + + + + + + + + + + + + + + + + + + catkin + Eigen3 + roscpp + wam_msgs + wam_srvs + eigen_conversions + Eigen3 + roscpp + wam_msgs + wam_srvs + eigen_conversions + Eigen3 + roscpp + wam_msgs + wam_srvs + eigen_conversions + + + + + + + + diff --git a/wam_teleoplib/src/wam_teleop.cpp b/wam_teleoplib/src/wam_teleop.cpp new file mode 100644 index 0000000..c7be961 --- /dev/null +++ b/wam_teleoplib/src/wam_teleop.cpp @@ -0,0 +1,367 @@ +/****************************************************************************** + WAM Teleop Library + Copyright (C) 2019 Walter Fetter Lages + + This program 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 program 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 program. If not, see + . + +*******************************************************************************/ + +#include + +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +namespace wam +{ + +WamTeleop::WamTeleop(const ros::NodeHandle &node,double updateRate,double jointTolerance,double jointRateLimit,double cartTolerance,double cartRateLimit): + runUpdate_(true),trackJointPos_(false),trackCartPos_(false),msgTimeout(0.4) +{ + node_=node; + jointPosPub_=node_.advertise("wam/jnt_pos_cmd",10); + cartPosPub_=node_.advertise("wam/cart_pos_cmd",10); + closeGraspCli_=node_.serviceClient("bhand/close_grasp"); + closeSpreadCli_=node_.serviceClient("bhand/close_spread"); + openGraspCli_=node_.serviceClient("bhand/open_grasp"); + openSpreadCli_=node_.serviceClient("bhand/open_spread"); + fingerPosCli_=node_.serviceClient("bhand/finger_pos"); + spreadPosCli_=node_.serviceClient("bhand/spread_pos"); + graspPosCli_=node_.serviceClient("bhand/grasp_pos"); + cartPosMoveCli_=node_.serviceClient("wam/cart_move"); + goHomeCli_=node_.serviceClient("wam/go_home"); + holdCartPosCli_=node_.serviceClient("wam/hold_cart_pos"); + holdJointPosCli_=node_.serviceClient("wam/hold_joint_pos"); + holdOrtnCli_=node_.serviceClient("wam/hold_ortn"); + jointMoveCli_=node_.serviceClient("wam/joint_move"); + ortnMoveCli_=node_.serviceClient("wam/ortn_move"); + poseMoveCli_=node_.serviceClient("wam/pose_move"); +// gravityCompCli_=node_.serviceClient("wam/gravity_comp"); + jointStatesSub_=node_.subscribe("/joint_states",10,&WamTeleop::jointStatesCB,this); + poseSub_=node_.subscribe("wam/pose",10,&WamTeleop::poseCB,this); + + jointTolerance_=jointTolerance; + updateRate_=updateRate; + jointRateLimit_=jointRateLimit; + cartTolerance_=cartTolerance; + cartRateLimit_=cartRateLimit; + + update_=std::thread(&WamTeleop::update,this); +} + +WamTeleop::~WamTeleop(void) +{ + runUpdate_=false; + update_.join(); + jointPosPub_.shutdown(); + cartPosPub_.shutdown(); + jointStatesSub_.shutdown(); + poseSub_.shutdown(); +} + +void WamTeleop::jointPos(const jp_type &jpCmd,bool block) +{ + jpCmd_=jpCmd; + trackJointPos(true); + if(block) jointWait(jpCmd); +} + +void WamTeleop::trackJointPos(bool track) +{ + trackJointPos_=track; +// ros::Duration(msgTimeout).sleep(); +} + +void WamTeleop::jointWait(const jp_type &jpCmd) const +{ + while((wamJp_-jpCmd).norm() > wamJp_.size()*jointTolerance_) + ros::Duration(0.01).sleep(); +} + +void WamTeleop::fingerPos(double f1,double f2,double f3,bool block) +{ + wam_srvs::BHandFingerPos fingerPosSrv; + + fingerPosSrv.request.radians[0]=f1; + fingerPosSrv.request.radians[1]=f2; + fingerPosSrv.request.radians[2]=f3; + if(fingerPosCli_.call(fingerPosSrv)) ROS_INFO_STREAM("Finger moved to " << f1 << " " << f2 << " " << f3 << "."); + else ROS_ERROR_STREAM("Error moving finger."); + if(block) fingerWait(f1,f2,f3); +} + +void WamTeleop::fingerWait(double f1,double f2,double f3) const +{ + Eigen::Vector3d jp(f1,f2,f3); + + while((jp-bhandJp_.head(3)).norm() > 3*jointTolerance_) + ros::Duration(0.01).sleep(); +} + +void WamTeleop::graspPos(double pos,bool block) +{ + wam_srvs::BHandGraspPos graspPosSrv; + + graspPosSrv.request.radians=pos; + if(graspPosCli_.call(graspPosSrv)) ROS_INFO_STREAM("Grasp moved to " << pos << "."); + else ROS_ERROR_STREAM("Error moving grasp."); + if(block) fingerWait(pos,pos,pos); +} + +void WamTeleop::spreadPos(double spread,bool block) +{ + wam_srvs::BHandSpreadPos spreadPosSrv; + + spreadPosSrv.request.radians=spread; + if(spreadPosCli_.call(spreadPosSrv)) ROS_INFO_STREAM("Spread moved to " << spread << "."); + else ROS_ERROR_STREAM("Error moving spread."); + if(block) spreadWait(spread); +} + +void WamTeleop::spreadWait(double spread) const +{ + while(fabs(spread-bhandJp_[3]) > jointTolerance_) + ros::Duration(0.01).sleep(); +} + +void WamTeleop::closeGrasp(bool block) +{ + std_srvs::Empty emptySrv; + + if(closeGraspCli_.call(emptySrv)) ROS_INFO_STREAM("Grasp closed."); + else ROS_ERROR_STREAM("Error closing grasp."); + if(block) fingerWait(2.44,2.44,2.44); +} + +void WamTeleop::closeSpread(bool block) +{ + std_srvs::Empty emptySrv; + + if(closeSpreadCli_.call(emptySrv)) ROS_INFO_STREAM("Spread closed."); + else ROS_ERROR_STREAM("Error closing spread."); + if(block) spreadWait(M_PI); +} + +void WamTeleop::openGrasp(bool block) +{ + std_srvs::Empty emptySrv; + + if(openGraspCli_.call(emptySrv)) ROS_INFO_STREAM("Grasp open."); + else ROS_ERROR_STREAM("Error opening grasp."); + if(block) fingerWait(0,0,0); +} + +void WamTeleop::openSpread(bool block) +{ + std_srvs::Empty emptySrv; + + if(openSpreadCli_.call(emptySrv)) ROS_INFO_STREAM("Spread open."); + else ROS_ERROR_STREAM("Error opening spread."); + if(block) spreadWait(0.0); +} + +void WamTeleop::goHome(void) +{ + std_srvs::Empty emptySrv; + + if(goHomeCli_.call(emptySrv)) ROS_INFO_STREAM("Returned home position."); + else ROS_ERROR_STREAM("Error returning home position."); +} + +void WamTeleop::holdCartPos(bool hold) +{ + wam_srvs::Hold holdSrv; + + holdSrv.request.hold=hold; + if(holdCartPosCli_.call(holdSrv)) ROS_INFO_STREAM("Cartesian position held."); + else ROS_ERROR_STREAM("Error holding Cartesian position."); +} + +void WamTeleop::holdJointPos(bool hold) +{ + wam_srvs::Hold holdSrv; + + holdSrv.request.hold=hold; + if(holdJointPosCli_.call(holdSrv)) ROS_INFO_STREAM("Joint position held."); + else ROS_ERROR_STREAM("Error holding joint position."); +} + +void WamTeleop::holdOrtnPos(bool hold) +{ + wam_srvs::Hold holdSrv; + + holdSrv.request.hold=hold; + if(holdOrtnCli_.call(holdSrv)) ROS_INFO_STREAM("Orientation held."); + else ROS_ERROR_STREAM("Error holding orientation."); +} + +void WamTeleop::jointMove(const jp_type &jpCmd,bool block) +{ + wam_srvs::JointMove jointMoveSrv; + + for(int i=0;i < jpCmd.size();i++) jointMoveSrv.request.joints.push_back(jpCmd[i]); + if(jointMoveCli_.call(jointMoveSrv)) + ROS_INFO_STREAM("Joints moved to " << jpCmd.transpose() << "."); + else ROS_ERROR_STREAM("Error moving joints."); + if(block) jointWait(jpCmd); +} + +void WamTeleop::cartPosMove(const cp_type &cpCmd,bool block) +{ + wam_srvs::CartPosMove cartPosMoveSrv; + + for(int i=0;i < 3;i++) cartPosMoveSrv.request.position[i]=cpCmd[i]; + if(cartPosMoveCli_.call(cartPosMoveSrv)) + ROS_INFO_STREAM("Cartesian posiiton moved to " << cpCmd.transpose() << "."); + else ROS_ERROR_STREAM("Error moving to Cartesian position."); + if(block) cartPosWait(cpCmd); +} + +void WamTeleop::cartPosWait(const cp_type &cpCmd) const +{ + while((wamCp_-cpCmd).norm() > cartTolerance_) + ros::Duration(0.01).sleep(); +} + +void WamTeleop::ortnMove(const Eigen::Quaterniond &ortnCmd,bool block) +{ + wam_srvs::OrtnMove ortnMoveSrv; + + ortnMoveSrv.request.orientation[0]=ortnCmd.x(); + ortnMoveSrv.request.orientation[1]=ortnCmd.y(); + ortnMoveSrv.request.orientation[2]=ortnCmd.z(); + ortnMoveSrv.request.orientation[3]=ortnCmd.w(); + if(ortnMoveCli_.call(ortnMoveSrv)) ROS_INFO_STREAM("Tool orientation moved to " + << ortnCmd.x() << " " << ortnCmd.y() << " " << ortnCmd.z() << " " << ortnCmd.w() << "."); + else ROS_ERROR_STREAM("Error moving tool orientation."); + if(block) ortnWait(ortnCmd); +} + +void WamTeleop::ortnWait(const Eigen::Quaterniond &ortnCmd) const +{ + while(fabs(ortnCmd.angularDistance(wamOrtn_)) > 8*jointTolerance_); + ros::Duration(0.01).sleep(); +} + +void WamTeleop::poseMove(const pose_type &poseCmd,bool block) +{ + wam_srvs::PoseMove poseMoveSrv; + + tf::pointEigenToMsg(std::get<0>(poseCmd),poseMoveSrv.request.pose.position); + tf::quaternionEigenToMsg(std::get<1>(poseCmd),poseMoveSrv.request.pose.orientation); + + if(poseMoveCli_.call(poseMoveSrv)) + ROS_INFO_STREAM("Pose moved to " << std::get<0>(poseCmd).transpose() << " " + << std::get<1>(poseCmd).x() << " " << std::get<1>(poseCmd).y() << " " << std::get<1>(poseCmd).z() << " " << std::get<1>(poseCmd).w() << "."); + else ROS_ERROR_STREAM("Error moving to pose."); + if(block) + { + cartPosWait(std::get<0>(poseCmd)); + ortnWait(std::get<1>(poseCmd)); + } +} + +void WamTeleop::cartPos(const cp_type &cpCmd,bool block) +{ + for(int i=0;i < cpCmd_.size();i++) cpCmd_[i]=cpCmd[i]; + + trackCartPos(true); + + if(block) cartPosWait(cpCmd); +} + +void WamTeleop::trackCartPos(bool track) +{ + trackCartPos_=track; +// ros::Duration(msgTimeout).sleep(); +} + +void WamTeleop::update(void) const +{ + ros::Rate loop(updateRate_); + while(ros::ok() && runUpdate_) + { + if(trackJointPos_) + { + wam_msgs::RTJointPos jointPosMsg; + + for(int i=0;i < jpCmd_.size();i++) jointPosMsg.joints.push_back(jpCmd_[i]); + jointPosMsg.rate_limits.resize(jpCmd_.size(),jointRateLimit_); + jointPosPub_.publish(jointPosMsg); + } + + if(trackCartPos_) + { + wam_msgs::RTCartPos cartPosMsg; + + for(int i=0;i < cpCmd_.size();i++) + { + cartPosMsg.position[i]=cpCmd_[i]; + cartPosMsg.rate_limits[i]=cartRateLimit_; + } + cartPosPub_.publish(cartPosMsg); + } + + ros::spinOnce(); + loop.sleep(); + } +} + +void WamTeleop::jointStatesCB(const sensor_msgs::JointState &jointStates) +{ + std::vector jointNames; + jointNames.push_back("wam_joint_1"); + jointNames.push_back("wam_joint_2"); + jointNames.push_back("wam_joint_3"); + jointNames.push_back("wam_joint_4"); + jointNames.push_back("wam_joint_5"); + jointNames.push_back("wam_joint_6"); + jointNames.push_back("wam_joint_7"); + jointNames.push_back("bhand_finger1_joint_2"); + jointNames.push_back("bhand_finger2_joint_2"); + jointNames.push_back("bhand_finger3_joint_2"); + jointNames.push_back("bhand_spread"); + + for(int i=0;i < 7;i++) + { + int j=find(jointStates.name.begin(),jointStates.name.end(),jointNames[i]) - jointStates.name.begin(); + wamJp_[i]=jointStates.position[j]; + } + + for(int i=0;i < 4;i++) + { + int j=find(jointStates.name.begin(),jointStates.name.end(),jointNames[i+7]) - jointStates.name.begin(); + bhandJp_[i]=jointStates.position[j]; + } +} + +void WamTeleop::poseCB(const geometry_msgs::PoseStamped &pose) +{ + tf::pointMsgToEigen(pose.pose.position,wamCp_); + tf::quaternionMsgToEigen(pose.pose.orientation,wamOrtn_); +} + +} // namespace wam