From: Walter Fetter Lages Date: Mon, 10 Dec 2018 08:57:28 +0000 (-0200) Subject: Add wam_node_test. X-Git-Url: http://git.ece.ufrgs.br/?a=commitdiff_plain;h=6f908347b75337f842daf4c21b20f52cad4d2c02;p=ufrgs_wam.git Add wam_node_test. --- diff --git a/wam_node_test/CMakeLists.txt b/wam_node_test/CMakeLists.txt new file mode 100644 index 0000000..42712a4 --- /dev/null +++ b/wam_node_test/CMakeLists.txt @@ -0,0 +1,199 @@ +cmake_minimum_required(VERSION 2.8.3) +project(wam_node_test) + +## 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_node + wam_node_sim +) + +## System dependencies are found with CMake's conventions +# find_package(Boost REQUIRED COMPONENTS system) + + +## 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 run_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 run_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 +# std_msgs # Or other packages containing msgs +# ) + +################################################ +## 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 run_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 you 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_node_test +# CATKIN_DEPENDS roscpp wam_node wam_node_sim +# DEPENDS system_lib +) + +########### +## Build ## +########### + +## Specify additional locations of header files +## Your package locations should be listed before other locations +include_directories( +# include + ${catkin_INCLUDE_DIRS} +) + +## Declare a C++ library +# add_library(${PROJECT_NAME} +# src/${PROJECT_NAME}/wam_node_test.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} src/wam_node_test.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} +) + +############# +## 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_node_test.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_node_test/package.xml b/wam_node_test/package.xml new file mode 100644 index 0000000..f1ba494 --- /dev/null +++ b/wam_node_test/package.xml @@ -0,0 +1,57 @@ + + + wam_node_test + 0.0.0 + The wam_node_test package + + + + + Walter Fetter Lages + + + + + + GPLv3 + + + + + + + + + + + + + Walter Fetter Lages + + + + + + + + + + + + + + catkin + roscpp + + wam_node_sim + roscpp + + wam_node_sim + + + + + + + + diff --git a/wam_node_test/src/wam_node_test.cpp b/wam_node_test/src/wam_node_test.cpp new file mode 100644 index 0000000..4ddc2c7 --- /dev/null +++ b/wam_node_test/src/wam_node_test.cpp @@ -0,0 +1,251 @@ +/****************************************************************************** + Node to test the wam_node + Copyright (C) 2018 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 + +#define sqr(x) ((x)*(x)) + +class WamNodeTest +{ + public: + WamNodeTest(const ros::NodeHandle &node); + ~WamNodeTest(void); + void wamMove(const std::vector &jpCmd); + void wamWait(const std::vector &jpCmd) const; + void fingerMove(double f1,double f2,double f3); + void fingerMove(double finger); + void fingerWait(double f1,double f2,double f3) const; + void fingerWait(double finger) const; + void spreadMove(double spread) + ; + void spreadWait(double spread) const; + void update(void) const; + void test(void); + + private: + + void jointStatesCB(const sensor_msgs::JointState &jointStates); + + ros::NodeHandle node_; + ros::Publisher jointPosPub_; + ros::Subscriber jointStatesSub_; + + ros::ServiceClient fingerPosCli_; + ros::ServiceClient spreadPosCli_; + + std::vector< std::atomic > jpCmd_; + std::vector< std::atomic > wamJp_; + std::vector< std::atomic > bhandJp_; + +}; + +WamNodeTest::WamNodeTest(const ros::NodeHandle &node):jpCmd_(7), wamJp_(7), bhandJp_(4) +{ + node_=node; + jointPosPub_=node_.advertise("wam/jnt_pos_cmd",10); + fingerPosCli_=node_.serviceClient("bhand/finger_pos"); + spreadPosCli_=node_.serviceClient("bhand/spread_pos"); + jointStatesSub_=node_.subscribe("/joint_states",10,&WamNodeTest::jointStatesCB,this); +} + +WamNodeTest::~WamNodeTest(void) +{ + jointPosPub_.shutdown(); + jointStatesSub_.shutdown(); +} + +void WamNodeTest::wamMove(const std::vector &jpCmd) +{ + for(int i=0;i < jpCmd_.size() && i < jpCmd.size();i++) jpCmd_[i]=jpCmd[i]; +} + +void WamNodeTest::wamWait(const std::vector &jpCmd) const +{ + float error; + do + { + ros::Duration(0.01).sleep(); + error=0.0; + for(int i=0;i < wamJp_.size() && i < jpCmd.size();i++) + error+=sqr(wamJp_[i]-jpCmd[i]); + } + while(error > 0.07); +} + +void WamNodeTest::fingerMove(double f1,double f2,double f3) +{ + 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 move to " << f1 << " " << f2 << " " << f3); + else ROS_ERROR_STREAM("Error in finger move."); +} + +void WamNodeTest::fingerWait(double f1,double f2,double f3) const +{ + float error; + do + { + ros::Duration(0.01).sleep(); + error=sqr(f1-bhandJp_[0])+sqr(f2-bhandJp_[1])+sqr(f3-bhandJp_[2]); + } + while(error > 0.03); +} + +void WamNodeTest::fingerWait(double finger) const +{ + fingerWait(finger,finger,finger); +} + + +void WamNodeTest::fingerMove(double finger) +{ + fingerMove(finger,finger,finger); +} + +void WamNodeTest::spreadMove(double spread) +{ + wam_srvs::BHandSpreadPos spreadPosSrv; + + spreadPosSrv.request.radians=spread; + if(spreadPosCli_.call(spreadPosSrv)) ROS_INFO_STREAM("Spread move to " << spread); + else ROS_ERROR_STREAM("Error in spread move."); +} + +void WamNodeTest::spreadWait(double spread) const +{ + float error; + do + { + ros::Duration(0.01).sleep(); + error=sqr(spread-bhandJp_[3]); + } + while(error > 0.01); +} + +void WamNodeTest::update(void) const +{ + wam_msgs::RTJointPos jointPosMsg; + + for(int i=0;i < jpCmd_.size();i++) jointPosMsg.joints.push_back(jpCmd_[i]); + jointPosMsg.rate_limits.resize(jpCmd_.size(),0.5); + jointPosPub_.publish(jointPosMsg); +} + +void WamNodeTest::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 WamNodeTest::test(void) +{ + std::vector jpCmd(7,0); + jpCmd[3]=M_PI_2; + wamMove(jpCmd); + wamWait(jpCmd); + + spreadMove(M_PI); + spreadWait(M_PI); + + fingerMove(M_PI_2); + fingerWait(M_PI_2); + + fingerMove(2.44); + fingerWait(2.44); + + fingerMove(0.0); + fingerWait(0.0); + + spreadMove(0); + spreadWait(0); + + fingerMove(2.44,2.44,0.0); + fingerWait(2.44,2.44,0.0); + + fingerMove(2.44); + fingerWait(2.44); + + fingerMove(0.0); + fingerWait(0.0); + + spreadMove(0.0); + spreadWait(0.0); + + std::fill(jpCmd.begin(),jpCmd.end(),0.0); + jpCmd[1]=-2.0; + jpCmd[3]=3.1; + wamMove(jpCmd); + wamWait(jpCmd); + + exit(0); +} + +int main(int argc,char* argv[]) +{ + ros::init(argc,argv,"wam_node_test"); + ros::NodeHandle node; + + WamNodeTest wamNodeTest(node); + std::thread test(&WamNodeTest::test,&wamNodeTest); + + ros::Rate loop(50); + while(ros::ok()) + { + wamNodeTest.update(); + ros::spinOnce(); + loop.sleep(); + } + + return 0; +}