From: Kyle Maroney Date: Thu, 16 Feb 2012 15:14:07 +0000 (+0000) Subject: ROS publisher added for /wam/wam_joints, /wam/wam_pose. Services added for /wam... X-Git-Url: http://git.ece.ufrgs.br/?a=commitdiff_plain;h=09f67b6e238ecf050610db250d8d06b21fd784b1;p=barrett-ros-pkg.git ROS publisher added for /wam/wam_joints, /wam/wam_pose. Services added for /wam/goHome, /wam/gravity_comp, /wam/joint_move/, /wam/cartesian_move. --- diff --git a/wam_node/CMakeLists.txt b/wam_node/CMakeLists.txt index 499c7b5..fe3e436 100644 --- a/wam_node/CMakeLists.txt +++ b/wam_node/CMakeLists.txt @@ -8,6 +8,9 @@ include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake) # RelWithDebInfo : w/ debug symbols, w/ optimization # MinSizeRel : w/o debug symbols, w/ optimization, stripped binaries #set(ROS_BUILD_TYPE RelWithDebInfo) +rosbuild_find_ros_package(actionlib_msgs) +include(${actionlib_msgs_PACKAGE_PATH}/cmake/actionbuild.cmake) +genaction() rosbuild_init() @@ -16,9 +19,7 @@ set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin) #set the default path for built libraries to the "lib" directory set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib) -#uncomment if you have defined messages rosbuild_genmsg() -#uncomment if you have defined services rosbuild_gensrv() link_directories(/usr/xenomai/lib /usr/local/lib) @@ -26,8 +27,8 @@ rosbuild_add_executable(wam_node src/wam_node.cpp) include_directories(/usr/xenomai/include /usr/local/include /usr/local/include/eigen2 /usr/local/include/boost) add_definitions(-D_GNU_SOURCE -D_REENTRANT -D__XENO__) rosbuild_add_compile_flags(wam_node -Wall -pipe) -target_link_libraries(wam_node barrett native xenomai pthread rt gsl gslcblas m) -rosbuild_add_boost_directories() -rosbuild_link_boost(wam_node thread ) +target_link_libraries(wam_node barrett native xenomai pthread rt gsl gslcblas m boost_signals boost_thread) +#rosbuild_add_boost_directories() +#rosbuild_link_boost(wam_node thread) diff --git a/wam_node/manifest.xml b/wam_node/manifest.xml index fb9fbf5..888de4a 100644 --- a/wam_node/manifest.xml +++ b/wam_node/manifest.xml @@ -10,7 +10,10 @@ http://ros.org/wiki/wam_node + + + diff --git a/wam_node/src/wam_node.cpp b/wam_node/src/wam_node.cpp index 16097a6..96c7a29 100644 --- a/wam_node/src/wam_node.cpp +++ b/wam_node/src/wam_node.cpp @@ -1,12 +1,12 @@ -#include -#include -#include // For mkstmp() -#include // For remove() - +#include #include "ros/ros.h" -#include "std_msgs/String.h" #include "wam_node/GravityComp.h" +#include "wam_node/JointMove.h" +#include "wam_node/CartMove.h" +#include "std_srvs/Empty.h" +#include "sensor_msgs/JointState.h" +#include "geometry_msgs/PoseStamped.h" #include #include @@ -15,55 +15,241 @@ #include #include +static const int PUBLISH_FREQ = 1; using namespace barrett; bool grav_state = true; +int wam_dof = 0; systems::Wam<4>* wam4 = NULL; systems::Wam<7>* wam7 = NULL; +units::JointPositions<4>::type* jp4; +units::JointPositions<4>::type* jp4home; +units::JointPositions<7>::type* jp7; +units::JointPositions<7>::type* jp7home; +units::CartesianPosition::type* cpp; +Eigen::Quaterniond to; +Eigen::Quaterniond tog; + +class WamNode +{ +protected: + //ros::NodeHandle n_; + sensor_msgs::JointState joint_state; + geometry_msgs::PoseStamped wam_pose; + ros::Publisher joint_pub; + ros::Publisher pose_pub; + ros::ServiceServer gravity_srv; + ros::ServiceServer jmove_srv; + ros::ServiceServer cmove_srv; + ros::ServiceServer home_srv; + +public: + WamNode(){} + + void init(std::vector &dof) + { + WamNode wam; + ros::NodeHandle n_("wam"); + wam_dof = dof.size(); + gravity_srv = n_.advertiseService("gravity_comp", &WamNode::gravity, &wam); + jmove_srv = n_.advertiseService("joint_move", &WamNode::joint_move, &wam); + cmove_srv = n_.advertiseService("cartesian_move", &WamNode::cart_move, &wam); + home_srv = n_.advertiseService("go_home",&WamNode::go_home, &wam); + joint_pub = n_.advertise("wam_joints", 100); + pose_pub = n_.advertise("wam_pose", 100); + + //explicitly naming joints until loaded from urdf + const char* strarray[] = {"j1", "j2", "j3", "j4", "j5", "j6", "j7"}; + std::vector joints(strarray, strarray + 7); + joint_state.name = joints; + joint_state.name.resize(wam_dof); + joint_state.position.resize(wam_dof); + joint_state.velocity.resize(wam_dof); + joint_state.effort.resize(wam_dof); + + if (wam_dof == 4) + { + *jp4home = wam4->getJointPositions(); + } + else if (wam_dof == 7) + { + *jp7home = wam7->getJointPositions(); + } + } + ~WamNode(){} -bool gravity(wam_node::GravityComp::Request &req, wam_node::GravityComp::Response &res) { - grav_state = req.gravity; + bool gravity(wam_node::GravityComp::Request &req, wam_node::GravityComp::Response &res); + bool joint_move(wam_node::JointMove::Request &req, wam_node::JointMove::Response &res); + bool cart_move(wam_node::CartMove::Request &req, wam_node::CartMove::Response &res); + bool go_home(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res); + void update_publisher(std::vector &j_pos, std::vector &j_vel, std::vector &j_tor, + std::vector &c_pos); +}; - if(wam4 != NULL) - wam4->gravityCompensate(grav_state); - else if(wam7 != NULL) - wam7->gravityCompensate(grav_state); +bool WamNode::gravity(wam_node::GravityComp::Request &req, wam_node::GravityComp::Response &res) +{ + grav_state = req.gravity; + if (wam4 != NULL) + wam4->gravityCompensate(grav_state); + else if (wam7 != NULL) + wam7->gravityCompensate(grav_state); - ROS_INFO("Gravity Compensation Request: %s",(req.gravity)?"true":"false"); + ROS_INFO("Gravity Compensation Request: %s", (req.gravity) ? "true" : "false"); return true; } -template -int wam_main(int argc, char** argv, ProductManager& pm, systems::Wam& wam) { - BARRETT_UNITS_TEMPLATE_TYPEDEFS(DOF); - wam.gravityCompensate(grav_state); - ros::init(argc, argv, "barrettWam"); - ros::NodeHandle nh_; - - if(pm.foundWam4()){ - wam4 = pm.getWam4(); - }else if(pm.foundWam7()) - wam7 = pm.getWam7(); - - std::cerr << "got Wam" << std::endl; - ros::Rate loop_rate(100); - ros::ServiceServer service = nh_.advertiseService("wam_gravity_comp",gravity); - while (ros::ok() && pm.getSafetyModule()->getMode() == SafetyModule::ACTIVE) +bool WamNode::joint_move(wam_node::JointMove::Request &req, wam_node::JointMove::Response &res) +{ + if (req.joints.size() != (size_t)wam_dof) { - ros::spinOnce(); - loop_rate.sleep(); + ROS_INFO("Request Failed: %d-DOF request received, must be %d-DOF", req.joints.size(), wam_dof); + return false; } - return 0; + if (wam_dof == 4) + { + for (int i = 0; i < 4; i++) + (*jp4)[i] = req.joints[i]; + wam4->moveTo(*jp4); + } + else + { + for (int j = 0; j < 7; j++) + (*jp7)[j] = req.joints[j]; + wam7->moveTo(*jp7); + } + ROS_INFO("Moving Robot to Commanded Joint Pose"); + return true; } +bool WamNode::cart_move(wam_node::CartMove::Request &req, wam_node::CartMove::Response &res) +{ + if (req.coordinates.size() != 3) + { + ROS_INFO("Request Failed: %d coordinate request received, must be 3 coordinates [X,Y,Z]", req.coordinates.size()); + return false; + } + for (int i = 0; i < 3; i++) + (*cpp)[i] = req.coordinates[i]; + if (wam_dof == 4) + wam4->moveTo(*jp4); + else + wam7->moveTo(*jp7); + ROS_INFO("Moving Robot to Commanded Coordinate Pose"); + return true; +} +bool WamNode::go_home(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res) +{ + if (wam_dof == 4) + { + wam4->moveTo(*jp4home); + } + else + { + wam7->moveTo(*jp7home); + } + ROS_INFO("Moving Robot to Starting Position"); + return true; +} +void WamNode::update_publisher(std::vector &j_pos, std::vector &j_vel, std::vector &j_tor, + std::vector &c_pos) +{ + //publishing sensor_msgs/JointState to wam_joints + for (size_t i = 0; i < j_pos.size(); i++) + { + joint_state.position[i] = j_pos[i]; + joint_state.velocity[i] = j_vel[i]; + joint_state.effort[i] = j_tor[i]; + } + joint_pub.publish(joint_state); + + //publishing geometry_msgs/PoseStamed to wam_pose + wam_pose.pose.position.x = c_pos[0]; + wam_pose.pose.position.y = c_pos[1]; + wam_pose.pose.position.z = c_pos[2]; + wam_pose.pose.orientation.w = to.w(); + wam_pose.pose.orientation.x = to.x(); + wam_pose.pose.orientation.y = to.y(); + wam_pose.pose.orientation.z = to.z(); + pose_pub.publish(wam_pose); +} - - +template + int wam_main(int argc, char** argv, ProductManager& pm, systems::Wam& wam) + { + BARRETT_UNITS_TEMPLATE_TYPEDEFS(DOF); + BARRETT_UNITS_FIXED_SIZE_TYPEDEFS; + wam.gravityCompensate(grav_state); + ros::init(argc, argv, "barrettWam"); + WamNode wam_node; + + if (pm.foundWam4()) + { + wam4 = pm.getWam4(); + } + else if (pm.foundWam7()) + wam7 = pm.getWam7(); + + jp_type jp; + jv_type jv; + jt_type jt; + cp_type cp; + + cpp = new units::CartesianPosition::type; + if (DOF == 4) + { + jp4 = new units::JointPositions<4>::type; + jp4home = new units::JointPositions<4>::type; + + } + else if (DOF == 7) + { + jp7 = new units::JointPositions<7>::type; + jp7home = new units::JointPositions<7>::type; + } + + std::vector joint_vel; + std::vector joint_tor; + std::vector joint_pos; + std::vector cart_pos; + joint_pos.resize(jp.size()); + joint_vel.resize(jv.size()); + joint_tor.resize(jt.size()); + cart_pos.resize(cp.size()); + + wam_node.init(joint_pos); + + int pub_rate = 1000000 * (1 / PUBLISH_FREQ); + while (ros::ok() && pm.getSafetyModule()->getMode() == SafetyModule::ACTIVE) + { + ros::spinOnce(); + jp = wam.getJointPositions(); + jt = wam.getJointTorques(); + jv = wam.getJointVelocities(); + cp = wam.getToolPosition(); + to = wam.getToolOrientation(); + for (size_t i = 0; i < DOF; i++) + { + joint_pos[i] = jp[i]; + joint_vel[i] = jv[i]; + joint_tor[i] = jt[i]; + if (i < 3) + cart_pos[i] = cp[i]; + } + wam_node.update_publisher(joint_pos, joint_vel, joint_tor, cart_pos); + usleep(pub_rate); // publishing at frequency PUBLISH_FREQ + } + delete cpp; + delete jp4; + delete jp4home; + delete jp7; + delete jp7home; + return 0; + }