## 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
catkin_package(
# INCLUDE_DIRS include
# LIBRARIES wam_node_test
-# CATKIN_DEPENDS roscpp wam_node_sim
+# CATKIN_DEPENDS roscpp wam_teleoplib
# DEPENDS system_lib
)
<!-- <doc_depend>doxygen</doc_depend> -->
<buildtool_depend>catkin</buildtool_depend>
<build_depend>roscpp</build_depend>
- <build_depend>wam_node_sim</build_depend>
+ <build_depend>wam_teleoplib</build_depend>
<build_export_depend>roscpp</build_export_depend>
- <build_export_depend>wam_node_sim</build_export_depend>
+ <build_export_depend>wam_teleoplib</build_export_depend>
<exec_depend>roscpp</exec_depend>
- <exec_depend>wam_node_sim</exec_depend>
+ <exec_depend>wam_teleoplib</exec_depend>
<!-- The export tag contains other, unspecified, tags -->
*******************************************************************************/
-#include <thread>
-
-#include <Eigen/Core>
-#include <Eigen/Geometry>
-
-#include <ros/ros.h>
-#include <eigen_conversions/eigen_msg.h>
-
-#include <geometry_msgs/PoseStamped.h>
-#include <sensor_msgs/JointState.h>
-#include <wam_msgs/RTJointPos.h>
-#include <wam_msgs/RTCartPos.h>
-
-#include <std_srvs/Empty.h>
-#include <wam_srvs/BHandFingerPos.h>
-#include <wam_srvs/BHandSpreadPos.h>
-#include <wam_srvs/BHandGraspPos.h>
-#include <wam_srvs/Hold.h>
-#include <wam_srvs/JointMove.h>
-#include <wam_srvs/CartPosMove.h>
-#include <wam_srvs/OrtnMove.h>
-#include <wam_srvs/PoseMove.h>
-
-class WamNodeOp
-{
- public:
- static const size_t DOF=7;
- typedef Eigen::Matrix<double,DOF,1> jt_type;
- typedef Eigen::Matrix<double,DOF,1> jp_type;
- typedef Eigen::Matrix<double,DOF,1> jv_type;
- typedef Eigen::Matrix<double,3,1> cp_type;
- typedef Eigen::Matrix<double,3,1> cv_type;
- typedef std::tuple<cp_type,Eigen::Quaterniond> 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_msgs::RTJointPos>("wam/jnt_pos_cmd",10);
- cartPosPub_=node_.advertise<wam_msgs::RTCartPos>("wam/cart_pos_cmd",10);
- closeGraspCli_=node_.serviceClient<std_srvs::Empty>("bhand/close_grasp");
- closeSpreadCli_=node_.serviceClient<std_srvs::Empty>("bhand/close_spread");
- openGraspCli_=node_.serviceClient<std_srvs::Empty>("bhand/open_grasp");
- openSpreadCli_=node_.serviceClient<std_srvs::Empty>("bhand/open_spread");
- fingerPosCli_=node_.serviceClient<wam_srvs::BHandFingerPos>("bhand/finger_pos");
- spreadPosCli_=node_.serviceClient<wam_srvs::BHandSpreadPos>("bhand/spread_pos");
- graspPosCli_=node_.serviceClient<wam_srvs::BHandGraspPos>("bhand/grasp_pos");
- cartPosMoveCli_=node_.serviceClient<wam_srvs::CartPosMove>("wam/cart_move");
- goHomeCli_=node_.serviceClient<std_srvs::Empty>("wam/go_home");
- holdCartPosCli_=node_.serviceClient<wam_srvs::Hold>("wam/hold_cart_pos");
- holdJointPosCli_=node_.serviceClient<wam_srvs::Hold>("wam/hold_joint_pos");
- holdOrtnCli_=node_.serviceClient<wam_srvs::Hold>("wam/hold_ortn");
- jointMoveCli_=node_.serviceClient<wam_srvs::JointMove>("wam/joint_move");
- ortnMoveCli_=node_.serviceClient<wam_srvs::OrtnMove>("wam/ortn_move");
- poseMoveCli_=node_.serviceClient<wam_srvs::PoseMove>("wam/pose_move");
-// gravityCompCli_=node_.serviceClient<wam_srvs::GravityComp>("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<std::string> 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 <wam_teleoplib/wam_teleop.h>
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;
wam.goHome();
- WamNodeOp::cp_type cpCmd;
+ wam::WamTeleop::cp_type cpCmd;
cpCmd[0]=0.0;
cpCmd[1]=-0.6;
cpCmd[2]=1.3;
--- /dev/null
+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)
--- /dev/null
+/******************************************************************************
+ WAM Teleop Library
+ Copyright (C) 2019 Walter Fetter Lages <w.fetter@ieee.org>
+
+ 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
+ <http://www.gnu.org/licenses/>.
+
+*******************************************************************************/
+
+#ifndef WAM_TELEOP_H
+#define WAM_TELEOP_H
+
+#include <thread>
+
+#include <Eigen/Core>
+#include <Eigen/Geometry>
+
+#include <ros/ros.h>
+
+#include <geometry_msgs/PoseStamped.h>
+#include <sensor_msgs/JointState.h>
+
+namespace wam
+{
+ class WamTeleop
+ {
+ public:
+ static const size_t DOF=7;
+ typedef Eigen::Matrix<double,DOF,1> jt_type;
+ typedef Eigen::Matrix<double,DOF,1> jp_type;
+ typedef Eigen::Matrix<double,DOF,1> jv_type;
+ typedef Eigen::Matrix<double,3,1> cp_type;
+ typedef Eigen::Matrix<double,3,1> cv_type;
+ typedef std::tuple<cp_type,Eigen::Quaterniond> 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
--- /dev/null
+<?xml version="1.0"?>
+<package format="2">
+ <name>wam_teleoplib</name>
+ <version>0.0.0</version>
+ <description>The wam_teleoplib package</description>
+
+ <!-- One maintainer tag required, multiple allowed, one person per tag -->
+ <!-- Example: -->
+ <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
+ <maintainer email="fetter@ece.ufrgs.br">Walter Fetter Lages</maintainer>
+
+
+ <!-- One license tag required, multiple allowed, one license per tag -->
+ <!-- Commonly used license strings: -->
+ <!-- BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
+ <license>GPLv3</license>
+
+
+ <!-- Url tags are optional, but multiple are allowed, one per tag -->
+ <!-- Optional attribute type can be: website, bugtracker, or repository -->
+ <!-- Example: -->
+ <!-- <url type="website">http://wiki.ros.org/wam_teleoplib</url> -->
+
+
+ <!-- Author tags are optional, multiple are allowed, one per tag -->
+ <!-- Authors do not have to be maintainers, but could be -->
+ <!-- Example: -->
+ <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
+ <author email="fetter@ece.ufrgs.br">Walter Fetter Lages</author>
+
+
+ <!-- The *depend tags are used to specify dependencies -->
+ <!-- Dependencies can be catkin packages or system dependencies -->
+ <!-- Examples: -->
+ <!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
+ <!-- <depend>roscpp</depend> -->
+ <!-- Note that this is equivalent to the following: -->
+ <!-- <build_depend>roscpp</build_depend> -->
+ <!-- <exec_depend>roscpp</exec_depend> -->
+ <!-- Use build_depend for packages you need at compile time: -->
+ <!-- <build_depend>message_generation</build_depend> -->
+ <!-- Use build_export_depend for packages you need in order to build against this package: -->
+ <!-- <build_export_depend>message_generation</build_export_depend> -->
+ <!-- Use buildtool_depend for build tool packages: -->
+ <!-- <buildtool_depend>catkin</buildtool_depend> -->
+ <!-- Use exec_depend for packages you need at runtime: -->
+ <!-- <exec_depend>message_runtime</exec_depend> -->
+ <!-- Use test_depend for packages you need only for testing: -->
+ <!-- <test_depend>gtest</test_depend> -->
+ <!-- Use doc_depend for packages you need only for building documentation: -->
+ <!-- <doc_depend>doxygen</doc_depend> -->
+ <buildtool_depend>catkin</buildtool_depend>
+ <build_depend>Eigen3</build_depend>
+ <build_depend>roscpp</build_depend>
+ <build_depend>wam_msgs</build_depend>
+ <build_depend>wam_srvs</build_depend>
+ <build_depend>eigen_conversions</build_depend>
+ <build_export_depend>Eigen3</build_export_depend>
+ <build_export_depend>roscpp</build_export_depend>
+ <build_export_depend>wam_msgs</build_export_depend>
+ <build_export_depend>wam_srvs</build_export_depend>
+ <build_export_depend>eigen_conversions</build_export_depend>
+ <exec_depend>Eigen3</exec_depend>
+ <exec_depend>roscpp</exec_depend>
+ <exec_depend>wam_msgs</exec_depend>
+ <exec_depend>wam_srvs</exec_depend>
+ <exec_depend>eigen_conversions</exec_depend>
+
+
+ <!-- The export tag contains other, unspecified, tags -->
+ <export>
+ <!-- Other tools can request additional information be placed here -->
+
+ </export>
+</package>
--- /dev/null
+/******************************************************************************
+ WAM Teleop Library
+ Copyright (C) 2019 Walter Fetter Lages <w.fetter@ieee.org>
+
+ 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
+ <http://www.gnu.org/licenses/>.
+
+*******************************************************************************/
+
+#include <eigen_conversions/eigen_msg.h>
+
+#include <wam_msgs/RTJointPos.h>
+#include <wam_msgs/RTCartPos.h>
+
+#include <std_srvs/Empty.h>
+#include <wam_srvs/BHandFingerPos.h>
+#include <wam_srvs/BHandSpreadPos.h>
+#include <wam_srvs/BHandGraspPos.h>
+#include <wam_srvs/Hold.h>
+#include <wam_srvs/JointMove.h>
+#include <wam_srvs/CartPosMove.h>
+#include <wam_srvs/OrtnMove.h>
+#include <wam_srvs/PoseMove.h>
+
+#include <wam_teleoplib/wam_teleop.h>
+
+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_msgs::RTJointPos>("wam/jnt_pos_cmd",10);
+ cartPosPub_=node_.advertise<wam_msgs::RTCartPos>("wam/cart_pos_cmd",10);
+ closeGraspCli_=node_.serviceClient<std_srvs::Empty>("bhand/close_grasp");
+ closeSpreadCli_=node_.serviceClient<std_srvs::Empty>("bhand/close_spread");
+ openGraspCli_=node_.serviceClient<std_srvs::Empty>("bhand/open_grasp");
+ openSpreadCli_=node_.serviceClient<std_srvs::Empty>("bhand/open_spread");
+ fingerPosCli_=node_.serviceClient<wam_srvs::BHandFingerPos>("bhand/finger_pos");
+ spreadPosCli_=node_.serviceClient<wam_srvs::BHandSpreadPos>("bhand/spread_pos");
+ graspPosCli_=node_.serviceClient<wam_srvs::BHandGraspPos>("bhand/grasp_pos");
+ cartPosMoveCli_=node_.serviceClient<wam_srvs::CartPosMove>("wam/cart_move");
+ goHomeCli_=node_.serviceClient<std_srvs::Empty>("wam/go_home");
+ holdCartPosCli_=node_.serviceClient<wam_srvs::Hold>("wam/hold_cart_pos");
+ holdJointPosCli_=node_.serviceClient<wam_srvs::Hold>("wam/hold_joint_pos");
+ holdOrtnCli_=node_.serviceClient<wam_srvs::Hold>("wam/hold_ortn");
+ jointMoveCli_=node_.serviceClient<wam_srvs::JointMove>("wam/joint_move");
+ ortnMoveCli_=node_.serviceClient<wam_srvs::OrtnMove>("wam/ortn_move");
+ poseMoveCli_=node_.serviceClient<wam_srvs::PoseMove>("wam/pose_move");
+// gravityCompCli_=node_.serviceClient<wam_srvs::GravityComp>("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<std::string> 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
projects / ufrgs_wam.git / commitdiff