From f8f631ca8885b5bf8673c6b772cc64675e431838 Mon Sep 17 00:00:00 2001 From: Walter Fetter Lages Date: Wed, 12 Dec 2018 16:43:04 -0200 Subject: [PATCH] Initial commit. --- .gitignore | 51 +++++++++++ CMakeLists.txt | 213 +++++++++++++++++++++++++++++++++++++++++++++ package.xml | 64 ++++++++++++++ src/joint_demux.cpp | 88 +++++++++++++++++++ src/pose_stamped2joint.cpp | 110 +++++++++++++++++++++++ src/pose_stamped2tf.cpp | 86 ++++++++++++++++++ 6 files changed, 612 insertions(+) create mode 100644 .gitignore create mode 100644 CMakeLists.txt create mode 100644 package.xml create mode 100644 src/joint_demux.cpp create mode 100644 src/pose_stamped2joint.cpp create mode 100644 src/pose_stamped2tf.cpp diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..c1edc29 --- /dev/null +++ b/.gitignore @@ -0,0 +1,51 @@ +devel/ +logs/ +build/ +bin/ +lib/ +msg_gen/ +srv_gen/ +msg/*Action.msg +msg/*ActionFeedback.msg +msg/*ActionGoal.msg +msg/*ActionResult.msg +msg/*Feedback.msg +msg/*Goal.msg +msg/*Result.msg +msg/_*.py +build_isolated/ +devel_isolated/ + +# Generated by dynamic reconfigure +*.cfgc +/cfg/cpp/ +/cfg/*.py + +# Ignore generated docs +#*.dox +*.wikidoc + +# eclipse stuff +.project +.cproject + +# qcreator stuff +CMakeLists.txt.user + +srv/_*.py +*.pcd +*.pyc +qtcreator-* +*.user + +/planning/cfg +/planning/docs +/planning/src + +*~ + +# Emacs +.#* + +# Catkin custom files +CATKIN_IGNORE diff --git a/CMakeLists.txt b/CMakeLists.txt new file mode 100644 index 0000000..9ce2bbc --- /dev/null +++ b/CMakeLists.txt @@ -0,0 +1,213 @@ +cmake_minimum_required(VERSION 2.8.3) +project(trajectory_conversions) + +## 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 + geometry_msgs + kdl_parser + kdl_conversions +# orocos_kdl + roscpp + std_msgs + trajectory_msgs +) + +## 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 trajectory_conversions +# CATKIN_DEPENDS geometry_msgs kdl_parser kdl_conversions orocos_kdl roscpp std_msgs trajectory_msgs +# 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}/trajectory_conversions.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(pose_stamped2joint src/pose_stamped2joint.cpp) +add_executable(pose_stamped2tf src/pose_stamped2tf.cpp) +add_executable(joint_demux src/joint_demux.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(pose_stamped2joint + ${catkin_LIBRARIES} +) +target_link_libraries(pose_stamped2tf + ${catkin_LIBRARIES} +) +target_link_libraries(joint_demux + ${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_trajectory_conversions.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/package.xml b/package.xml new file mode 100644 index 0000000..a60a4a4 --- /dev/null +++ b/package.xml @@ -0,0 +1,64 @@ + + + trajectory_conversions + 0.0.0 + The trajectory_conversions package + + + + + Walter Fetter Lages + + + + + + GPLv3 + + + + + + + + + + + + + Walter Fetter Lages + + + + + + + + + + + + + + catkin + geometry_msgs + kdl_parser + kdl_conversions + orocos_kdl + roscpp + std_msgs + trajectory_msgs + geometry_msgs + kdl_parser + kdl_conversions + orocos_kdl + roscpp + std_msgs + trajectory_msgs + + + + + + + diff --git a/src/joint_demux.cpp b/src/joint_demux.cpp new file mode 100644 index 0000000..d7a64ec --- /dev/null +++ b/src/joint_demux.cpp @@ -0,0 +1,88 @@ +/* + joint_demux: Demux a Joint Trajectory Point in separate independent commands + + 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 2 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, write to the Free Software + Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA. + + You can also obtain a copy of the GNU General Public License + at . + +*/ + +#include +#include +#include +#include + +class JointDemux +{ + public: + JointDemux(ros::NodeHandle node); + ~JointDemux(void); + + private: + ros::NodeHandle node_; + + ros::Subscriber jointTrajPointSub_; + std::vector jointPosPub_; + + void jointTrajPointCB(const trajectory_msgs::JointTrajectoryPoint &jointTrajPoint); +}; + +JointDemux::JointDemux(ros::NodeHandle node) +{ + node_=node; + + jointTrajPointSub_=node_.subscribe("joint_trajectory_point",10,&JointDemux::jointTrajPointCB,this); +} + +JointDemux::~JointDemux(void) +{ + jointTrajPointSub_.shutdown(); + for(int i=jointPosPub_.size()-1;i >= 0;i--) + { + jointPosPub_[i].shutdown(); + jointPosPub_.pop_back(); + } +} + +void JointDemux::jointTrajPointCB(const trajectory_msgs::JointTrajectoryPoint &jointTrajPoint) +{ + for(int i=jointPosPub_.size();i < jointTrajPoint.positions.size();i++) + { + ros::Publisher jointPosPub; + jointPosPub=node_.advertise("position"+std::to_string(i),10); + jointPosPub_.push_back(jointPosPub); + } + for(int i=0;i < jointTrajPoint.positions.size() && i < jointPosPub_.size();i++) + { + std_msgs::Float64 cmdMsg; + cmdMsg.data=jointTrajPoint.positions[i]; + jointPosPub_[i].publish(cmdMsg); + } +} + +int main(int argc,char* argv[]) +{ + ros::init(argc,argv,"joint_demux"); + ros::NodeHandle node; + + JointDemux pose2joint(node); + + ros::spin(); + + return 0; +} diff --git a/src/pose_stamped2joint.cpp b/src/pose_stamped2joint.cpp new file mode 100644 index 0000000..486c044 --- /dev/null +++ b/src/pose_stamped2joint.cpp @@ -0,0 +1,110 @@ +/* + pose_stamped2joint: A ROS node to map Cartesian pose to joint space + + 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 2 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, write to the Free Software + Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA. + + You can also obtain a copy of the GNU General Public License + at . + +*/ + +#include +#include +#include +#include +#include +#include + +class Pose2Joint +{ + public: + Pose2Joint(ros::NodeHandle node,const std::string &root,const std::string &tip,const Eigen::Matrix &L); + ~Pose2Joint(void); + + private: + ros::NodeHandle node_; + + ros::Subscriber poseSub_; + ros::Publisher jointTrajPointPub_; + + KDL::Chain chain_; + KDL::ChainIkSolverPos_LMA *ikSolverPos_; + KDL::JntArray q_; + ros::Time t0_; + + void poseCB(const geometry_msgs::PoseStamped &pose); +}; + +Pose2Joint::Pose2Joint(ros::NodeHandle node,const std::string &root,const std::string &tip,const Eigen::Matrix &L):q_(0) +{ + node_=node; + + KDL::Tree tree; + if(!kdl_parser::treeFromParam("robot_description",tree)) ROS_ERROR_STREAM("Failed to construct KDL tree."); + if(!tree.getChain(root,tip,chain_)) ROS_ERROR_STREAM("Failed to get chain from KDL tree."); + ikSolverPos_=new KDL::ChainIkSolverPos_LMA(chain_,L); + + q_.resize(chain_.getNrOfJoints()); + t0_=ros::Time::now(); + + jointTrajPointPub_=node_.advertise("joint_trajectory_point",10); + poseSub_=node_.subscribe("/pose",10,&Pose2Joint::poseCB,this); +} + +Pose2Joint::~Pose2Joint(void) +{ + poseSub_.shutdown(); + jointTrajPointPub_.shutdown(); + delete ikSolverPos_; +} + +void Pose2Joint::poseCB(const geometry_msgs::PoseStamped &poseStamped) +{ + KDL::Frame goalFrame; + tf::poseMsgToKDL(poseStamped.pose,goalFrame); + + int error=ikSolverPos_->CartToJnt(q_,goalFrame,q_); + if(error != 0) ROS_ERROR_STREAM("Failed to compute invere kinematics: (" << error << ") " + << ikSolverPos_->strError(error)); + + trajectory_msgs::JointTrajectoryPoint jointTrajPoint; + jointTrajPoint.positions.resize(q_.data.size()); + Eigen::VectorXd::Map(&jointTrajPoint.positions[0],jointTrajPoint.positions.size())=q_.data; + jointTrajPoint.time_from_start=poseStamped.header.stamp-t0_;; + jointTrajPointPub_.publish(jointTrajPoint); +} + +int main(int argc,char* argv[]) +{ + ros::init(argc,argv,"pose_stamped2joint"); + if(argc < 3) + { + ROS_ERROR_STREAM("Please, provide a chain root and a chain tip"); + return -1; + } + ros::NodeHandle node; + + Eigen::Matrix L; + L << 1.0 , 1.0 , 1.0, 0.01, 0.01, 0.01; + for(int i=0;i < argc-3 && i < L.size();i++) L(i)=atof(argv[i+3]); + + Pose2Joint pose2joint(node,argv[1],argv[2],L); + + ros::spin(); + + return 0; +} diff --git a/src/pose_stamped2tf.cpp b/src/pose_stamped2tf.cpp new file mode 100644 index 0000000..461d32d --- /dev/null +++ b/src/pose_stamped2tf.cpp @@ -0,0 +1,86 @@ +/****************************************************************************** + ROS PoseStamped to tf Converter + 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 + +class PoseStampedToTf +{ + public: + PoseStampedToTf(const ros::NodeHandle &node,const char *child_frame_id); + ~PoseStampedToTf(void); + + private: + ros::NodeHandle node_; + ros::Subscriber poseSubscriber_; + ros::Publisher tfPublisher_; + std::string child_frame_id_; + void poseStampedCB(const geometry_msgs::PoseStamped::ConstPtr &poseMsg) const; +}; + +PoseStampedToTf::PoseStampedToTf(const ros::NodeHandle &node,const char *child_frame_id) +{ + node_=node; + child_frame_id_=child_frame_id; + poseSubscriber_=node_.subscribe("pose",10,&PoseStampedToTf::poseStampedCB,this); + tfPublisher_=node_.advertise("/tf",10); +} + +PoseStampedToTf::~PoseStampedToTf(void) +{ + poseSubscriber_.shutdown(); + tfPublisher_.shutdown(); +} + +void PoseStampedToTf::poseStampedCB(const geometry_msgs::PoseStamped::ConstPtr &poseMsg) const +{ + tf::tfMessage tfMsg; + + tfMsg.transforms.resize(1); + tfMsg.transforms[0].header.stamp=poseMsg->header.stamp; + tfMsg.transforms[0].header.frame_id=poseMsg->header.frame_id; + tfMsg.transforms[0].child_frame_id=child_frame_id_; + tfMsg.transforms[0].transform.translation.x=poseMsg->pose.position.x; + tfMsg.transforms[0].transform.translation.y=poseMsg->pose.position.y; + tfMsg.transforms[0].transform.translation.z=poseMsg->pose.position.z; + tfMsg.transforms[0].transform.rotation=poseMsg->pose.orientation; + + tfPublisher_.publish(tfMsg); +} + +int main(int argc,char* argv[]) +{ + ros::init(argc,argv,"pose_stamped2tf"); + ros::NodeHandle node; + + if(argc != 2) + { + ROS_ERROR_STREAM("pose_stamped2tf: No child frame id.\n"); + return -1; + } + + PoseStampedToTf poseStampedToTf(node,argv[1]); + + ros::spin(); + + return 0; +} -- 2.12.0