-cmake_minimum_required(VERSION 3.0.2)
+cmake_minimum_required(VERSION 3.8)
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_conversions
- kdl_parser
- roscpp
- trajectory_msgs
-)
-
-## System dependencies are found with CMake's conventions
-# find_package(Boost REQUIRED COMPONENTS system)
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+ add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+# find dependencies
+find_package(ament_cmake REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(std_msgs REQUIRED)
+find_package(geometry_msgs REQUIRED)
+find_package(trajectory_msgs REQUIRED)
+find_package(tf2_kdl REQUIRED)
+find_package(urdf REQUIRED)
+find_package(kdl_parser REQUIRED)
find_package(orocos_kdl 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
-# geometry_msgs# trajectory_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 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 trajectory_conversions
-# CATKIN_DEPENDS geometry_msgs kdl_conversions kdl_parser roscpp trajectory_msgs
-# DEPENDS orocos_kdl
-)
-
-###########
-## Build ##
-###########
-
-## Specify additional locations of header files
-## Your package locations should be listed before other locations
-include_directories(
-# include
- ${catkin_INCLUDE_DIRS}
-# TODO: Check names of system library include directories (orocos_kdl)
- ${orocos_kdl_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)
-
-## 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}
- ${orocos_kdl_LIBRARIES}
+target_include_directories(pose_stamped2joint PUBLIC
+ $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+ $<INSTALL_INTERFACE:include>)
+target_compile_features(pose_stamped2joint PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17
+ament_target_dependencies(
+ pose_stamped2joint
+ "rclcpp"
+ "std_msgs"
+ "geometry_msgs"
+ "trajectory_msgs"
+ "tf2_kdl"
+ "urdf"
+ "kdl_parser"
+ "orocos_kdl"
)
-
-#############
-## 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
-# catkin_install_python(PROGRAMS
-# scripts/my_python_script
-# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
-# )
-
-## Mark executables for installation
-## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html
-# install(TARGETS ${PROJECT_NAME}_node
-# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
-# )
-
-## Mark libraries for installation
-## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html
-# install(TARGETS ${PROJECT_NAME}
-# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-# RUNTIME DESTINATION ${CATKIN_GLOBAL_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)
+install(TARGETS pose_stamped2joint
+ DESTINATION lib/${PROJECT_NAME})
+
+if(BUILD_TESTING)
+ find_package(ament_lint_auto REQUIRED)
+ # the following line skips the linter which checks for copyrights
+ # uncomment the line when a copyright and license is not present in all source files
+ #set(ament_cmake_copyright_FOUND TRUE)
+ # the following line skips cpplint (only works in a git repo)
+ # uncomment the line when this package is not in a git repo
+ #set(ament_cmake_cpplint_FOUND TRUE)
+ ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_package()
<?xml version="1.0"?>
-<package format="2">
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
<name>trajectory_conversions</name>
- <version>0.1.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> -->
+ <version>2.0.0</version>
+ <description>Trajectory Conversions</description>
<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>
+ <buildtool_depend>ament_cmake</buildtool_depend>
- <!-- 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> -->
-
+ <depend>rclcpp</depend>
+ <depend>std_msgs</depend>
+ <depend>geometry_msgs</depend>
+ <depend>trajectory_msgs</depend>
+ <depend>urdf</depend>
+ <depend>tf2_kdl</depend>
+ <depend>kdl_parser</depend>
+ <depend>orocos_kdl</depend>
- <!-- 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>
+ <test_depend>ament_lint_auto</test_depend>
+ <test_depend>ament_lint_common</test_depend>
-
- <!-- 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>geometry_msgs</build_depend>
- <build_depend>kdl_conversions</build_depend>
- <build_depend>kdl_parser</build_depend>
- <build_depend>orocos_kdl</build_depend>
- <build_depend>roscpp</build_depend>
- <build_depend>trajectory_msgs</build_depend>
- <build_export_depend>geometry_msgs</build_export_depend>
- <build_export_depend>kdl_conversions</build_export_depend>
- <build_export_depend>kdl_parser</build_export_depend>
- <build_export_depend>orocos_kdl</build_export_depend>
- <build_export_depend>roscpp</build_export_depend>
- <build_export_depend>trajectory_msgs</build_export_depend>
- <exec_depend>geometry_msgs</exec_depend>
- <exec_depend>kdl_conversions</exec_depend>
- <exec_depend>kdl_parser</exec_depend>
- <exec_depend>orocos_kdl</exec_depend>
- <exec_depend>roscpp</exec_depend>
- <exec_depend>trajectory_msgs</exec_depend>
-
-
- <!-- The export tag contains other, unspecified, tags -->
<export>
- <!-- Other tools can request additional information be placed here -->
-
+ <build_type>ament_cmake</build_type>
</export>
</package>
/*
- pose_stamped2joint: A ROS node to map Cartesian pose to joint space
+ pose_stamped2joint: A ROS 2 node to map Cartesian pose to joint space
- Copyright (c) 2018 Walter Fetter Lages <w.fetter@ieee.org>
+ Copyright (c) 2018, 2021 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
*/
-#include <ros/ros.h>
-#include <geometry_msgs/PoseStamped.h>
-#include <trajectory_msgs/JointTrajectoryPoint.h>
+#include <rclcpp/rclcpp.hpp>
+#include <geometry_msgs/msg/point_stamped.hpp>
+#include <std_msgs/msg/string.hpp>
+#include <trajectory_msgs/msg/joint_trajectory_point.hpp>
#include <kdl/chainiksolverpos_lma.hpp>
-#include <kdl_conversions/kdl_msg.h>
+#include <tf2_kdl/tf2_kdl.h>
#include <kdl_parser/kdl_parser.hpp>
-class Pose2Joint
+class Pose2Joint: public rclcpp::Node
{
public:
- Pose2Joint(ros::NodeHandle node,const std::string &root,const std::string &tip,const Eigen::Matrix<double,6,1> &L);
- ~Pose2Joint(void);
+ Pose2Joint(const std::string &name,const std::string &root,const std::string &tip,const Eigen::Matrix<double,6,1> &L);
private:
- ros::NodeHandle node_;
-
- ros::Subscriber poseSub_;
- ros::Publisher jointTrajPointPub_;
+ rclcpp::Subscription<geometry_msgs::msg::PoseStamped>::SharedPtr poseSub_;
+ rclcpp::Publisher<trajectory_msgs::msg::JointTrajectoryPoint>::SharedPtr jointTrajPointPub_;
+ std::string robotDescription_;
KDL::Chain chain_;
- KDL::ChainIkSolverPos_LMA *ikSolverPos_;
+ std::unique_ptr<KDL::ChainIkSolverPos_LMA> ikSolverPos_;
KDL::JntArray q_;
- ros::Time t0_;
+ builtin_interfaces::msg::Time::UniquePtr t0_;
- void poseCB(const geometry_msgs::PoseStamped &pose);
+ void poseCB(const geometry_msgs::msg::PoseStamped::SharedPtr pose);
+ void robotDescriptionCB(const std_msgs::msg::String::SharedPtr robotDescription);
};
-Pose2Joint::Pose2Joint(ros::NodeHandle node,const std::string &root,const std::string &tip,const Eigen::Matrix<double,6,1> &L):q_(0)
+Pose2Joint::Pose2Joint(const std::string &name,const std::string &root,const std::string &tip,const Eigen::Matrix<double,6,1> &L): Node(name), q_(0)
{
- node_=node;
-
+ using std::placeholders::_1;
+ rclcpp::QoS qos(rclcpp::KeepLast(1));
+ qos.transient_local();
+ auto robotDescriptionSubscriber_=create_subscription<std_msgs::msg::String>("robot_description",qos,std::bind(&Pose2Joint::robotDescriptionCB,this,_1));
+ while(robotDescription_.empty())
+ {
+ RCLCPP_WARN_STREAM_SKIPFIRST_THROTTLE(get_logger(),*get_clock(),1000,"Waiting for robot model on /robot_description.");
+ rclcpp::spin_some(get_node_base_interface());
+ }
+
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);
+ if(!kdl_parser::treeFromString(robotDescription_,tree)) RCLCPP_ERROR_STREAM(get_logger(),"Failed to construct KDL tree.");
+ if(!tree.getChain(root,tip,chain_)) RCLCPP_ERROR_STREAM(get_logger(),"Failed to get chain from KDL tree.");
+ ikSolverPos_=std::make_unique<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_;
+ jointTrajPointPub_=create_publisher<trajectory_msgs::msg::JointTrajectoryPoint>("joint_trajectory_point",10);
+ poseSub_=create_subscription<geometry_msgs::msg::PoseStamped>("/pose",10,std::bind(&Pose2Joint::poseCB,this,_1));
}
-void Pose2Joint::poseCB(const geometry_msgs::PoseStamped &poseStamped)
+void Pose2Joint::poseCB(const geometry_msgs::msg::PoseStamped::SharedPtr poseStamped)
{
- KDL::Frame goalFrame;
- tf::poseMsgToKDL(poseStamped.pose,goalFrame);
+ tf2::Stamped<KDL::Frame> goalFrame;
+ tf2::fromMsg(*poseStamped,goalFrame);
int error=ikSolverPos_->CartToJnt(q_,goalFrame,q_);
- if(error != 0) ROS_ERROR_STREAM("Failed to compute invere kinematics: (" << error << ") "
+ if(error != 0) RCLCPP_ERROR_STREAM(get_logger(),"Failed to compute invere kinematics: (" << error << ") "
<< ikSolverPos_->strError(error));
- trajectory_msgs::JointTrajectoryPoint jointTrajPoint;
- jointTrajPoint.positions.resize(q_.data.size());
+ trajectory_msgs::msg::JointTrajectoryPoint jointTrajPoint;
+ jointTrajPoint.positions.resize(q_.rows());
Eigen::VectorXd::Map(&jointTrajPoint.positions[0],jointTrajPoint.positions.size())=q_.data;
- jointTrajPoint.time_from_start=poseStamped.header.stamp-t0_;;
- jointTrajPointPub_.publish(jointTrajPoint);
+
+ if(!t0_) t0_=std::make_unique<builtin_interfaces::msg::Time>(poseStamped->header.stamp);
+ jointTrajPoint.time_from_start.sec=poseStamped->header.stamp.sec-t0_->sec;
+ jointTrajPoint.time_from_start.nanosec=poseStamped->header.stamp.nanosec-t0_->nanosec;
+
+ jointTrajPointPub_->publish(jointTrajPoint);
+}
+
+void Pose2Joint::robotDescriptionCB(const std_msgs::msg::String::SharedPtr robotDescription)
+{
+ robotDescription_=robotDescription->data;
}
int main(int argc,char* argv[])
{
- ros::init(argc,argv,"pose_stamped2joint");
+ rclcpp::init(argc,argv);
if(argc < 3)
{
- ROS_ERROR_STREAM("Please, provide a chain root and a chain tip");
+ RCLCPP_ERROR_STREAM(rclcpp::get_logger("pose_stamped2joint"),"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();
+ rclcpp::spin(std::make_shared<Pose2Joint>("pose_stamped2joint",argv[1],argv[2],L));
return 0;
}
projects / trajectory_conversions.git / commitdiff