From: Walter Fetter Lages Date: Wed, 20 Oct 2021 16:32:35 +0000 (-0300) Subject: Port to Galactic. X-Git-Url: http://git.ece.ufrgs.br/?a=commitdiff_plain;h=1918c3c476aa0dbd494c5e6bfff0126a5d1a2836;p=trajectory_conversions.git Port to Galactic. --- diff --git a/CMakeLists.txt b/CMakeLists.txt index 9a9c2b1..cce1fd2 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,213 +1,50 @@ -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 + $ + $) +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() diff --git a/package.xml b/package.xml index 774666d..903c0d4 100644 --- a/package.xml +++ b/package.xml @@ -1,78 +1,27 @@ - + + trajectory_conversions - 0.1.0 - The trajectory_conversions package - - - - + 2.0.0 + Trajectory Conversions Walter Fetter Lages - - - - - GPLv3 + ament_cmake - - - - - + rclcpp + std_msgs + geometry_msgs + trajectory_msgs + urdf + tf2_kdl + kdl_parser + orocos_kdl - - - - - Walter Fetter Lages + ament_lint_auto + ament_lint_common - - - - - - - - - - - - - - - - - - - - - - catkin - geometry_msgs - kdl_conversions - kdl_parser - orocos_kdl - roscpp - trajectory_msgs - geometry_msgs - kdl_conversions - kdl_parser - orocos_kdl - roscpp - trajectory_msgs - geometry_msgs - kdl_conversions - kdl_parser - orocos_kdl - roscpp - trajectory_msgs - - - - - + ament_cmake diff --git a/src/pose_stamped2joint.cpp b/src/pose_stamped2joint.cpp index 486c044..2ab63cb 100644 --- a/src/pose_stamped2joint.cpp +++ b/src/pose_stamped2joint.cpp @@ -1,7 +1,7 @@ /* - 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 + Copyright (c) 2018, 2021 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 @@ -22,89 +22,95 @@ */ -#include -#include -#include +#include +#include +#include +#include #include -#include +#include #include -class Pose2Joint +class Pose2Joint: public rclcpp::Node { public: - Pose2Joint(ros::NodeHandle node,const std::string &root,const std::string &tip,const Eigen::Matrix &L); - ~Pose2Joint(void); + Pose2Joint(const std::string &name,const std::string &root,const std::string &tip,const Eigen::Matrix &L); private: - ros::NodeHandle node_; - - ros::Subscriber poseSub_; - ros::Publisher jointTrajPointPub_; + rclcpp::Subscription::SharedPtr poseSub_; + rclcpp::Publisher::SharedPtr jointTrajPointPub_; + std::string robotDescription_; KDL::Chain chain_; - KDL::ChainIkSolverPos_LMA *ikSolverPos_; + std::unique_ptr 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 &L):q_(0) +Pose2Joint::Pose2Joint(const std::string &name,const std::string &root,const std::string &tip,const Eigen::Matrix &L): Node(name), q_(0) { - node_=node; - + using std::placeholders::_1; + rclcpp::QoS qos(rclcpp::KeepLast(1)); + qos.transient_local(); + auto robotDescriptionSubscriber_=create_subscription("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(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_; + jointTrajPointPub_=create_publisher("joint_trajectory_point",10); + poseSub_=create_subscription("/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 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(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 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("pose_stamped2joint",argv[1],argv[2],L)); return 0; }