--- /dev/null
+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
--- /dev/null
+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)
--- /dev/null
+<?xml version="1.0"?>
+<package>
+ <name>trajectory_conversions</name>
+ <version>0.0.0</version>
+ <description>The trajectory_conversions 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/trajectory_conversions</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 build_depend for packages you need at compile time: -->
+ <!-- <build_depend>message_generation</build_depend> -->
+ <!-- Use buildtool_depend for build tool packages: -->
+ <!-- <buildtool_depend>catkin</buildtool_depend> -->
+ <!-- Use run_depend for packages you need at runtime: -->
+ <!-- <run_depend>message_runtime</run_depend> -->
+ <!-- Use test_depend for packages you need only for testing: -->
+ <!-- <test_depend>gtest</test_depend> -->
+ <buildtool_depend>catkin</buildtool_depend>
+ <build_depend>geometry_msgs</build_depend>
+ <build_depend>kdl_parser</build_depend>
+ <build_depend>kdl_conversions</build_depend>
+ <build_depend>orocos_kdl</build_depend>
+ <build_depend>roscpp</build_depend>
+ <build_depend>std_msgs</build_depend>
+ <build_depend>trajectory_msgs</build_depend>
+ <run_depend>geometry_msgs</run_depend>
+ <run_depend>kdl_parser</run_depend>
+ <run_depend>kdl_conversions</run_depend>
+ <run_depend>orocos_kdl</run_depend>
+ <run_depend>roscpp</run_depend>
+ <run_depend>std_msgs</run_depend>
+ <run_depend>trajectory_msgs</run_depend>
+
+ <!-- The export tag contains other, unspecified, tags -->
+ <export>
+ <!-- Other tools can request additional information be placed here -->
+
+ </export>
+</package>
--- /dev/null
+/*
+ joint_demux: Demux a Joint Trajectory Point in separate independent commands
+
+ Copyright (c) 2018 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 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 <http://www.gnu.org/licenses>.
+
+*/
+
+#include <string>
+#include <ros/ros.h>
+#include <std_msgs/Float64.h>
+#include <trajectory_msgs/JointTrajectoryPoint.h>
+
+class JointDemux
+{
+ public:
+ JointDemux(ros::NodeHandle node);
+ ~JointDemux(void);
+
+ private:
+ ros::NodeHandle node_;
+
+ ros::Subscriber jointTrajPointSub_;
+ std::vector<ros::Publisher> 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<std_msgs::Float64>("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;
+}
--- /dev/null
+/*
+ pose_stamped2joint: A ROS node to map Cartesian pose to joint space
+
+ Copyright (c) 2018 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 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 <http://www.gnu.org/licenses>.
+
+*/
+
+#include <ros/ros.h>
+#include <geometry_msgs/PoseStamped.h>
+#include <trajectory_msgs/JointTrajectoryPoint.h>
+#include <kdl/chainiksolverpos_lma.hpp>
+#include <kdl_conversions/kdl_msg.h>
+#include <kdl_parser/kdl_parser.hpp>
+
+class Pose2Joint
+{
+ public:
+ Pose2Joint(ros::NodeHandle node,const std::string &root,const std::string &tip,const Eigen::Matrix<double,6,1> &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<double,6,1> &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<trajectory_msgs::JointTrajectoryPoint>("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<double,6,1> 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;
+}
--- /dev/null
+/******************************************************************************
+ ROS PoseStamped to tf Converter
+ Copyright (C) 2018 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 <ros/ros.h>
+
+#include <geometry_msgs/PoseStamped.h>
+#include <tf/tfMessage.h>
+
+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::tfMessage>("/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;
+}
projects / trajectory_conversions.git / commitdiff