find_package(geometry_msgs REQUIRED)
find_package(tf2_msgs REQUIRED)
find_package(trajectory_msgs REQUIRED)
-find_package(tf2_kdl REQUIRED)
find_package(urdf REQUIRED)
+find_package(tf2_kdl REQUIRED)
find_package(kdl_parser REQUIRED)
find_package(orocos_kdl REQUIRED)
"rclcpp"
"std_msgs"
"geometry_msgs"
+ "tf2_msgs"
"trajectory_msgs"
- "tf2_kdl"
"urdf"
+ "tf2_kdl"
"kdl_parser"
"orocos_kdl"
)
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
+ # comment the line when a copyright and license is added to 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
+ # comment the line when this package is in a git repo and when
+ # a copyright and license is added to all source files
#set(ament_cmake_cpplint_FOUND TRUE)
ament_lint_auto_find_test_dependencies()
endif()
<?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>2.0.0</version>
- <description>Trajectory Conversions</description>
+ <version>2.1.0</version>
+ <description>Trajectory conversions</description>
<maintainer email="fetter@ece.ufrgs.br">Walter Fetter Lages</maintainer>
- <license>GPLv3</license>
+ <license>GPL-3.0-only</license>
<buildtool_depend>ament_cmake</buildtool_depend>
#include <std_msgs/msg/string.hpp>
#include <trajectory_msgs/msg/joint_trajectory_point.hpp>
#include <kdl/chainiksolverpos_lma.hpp>
-#include <tf2_kdl/tf2_kdl.h>
+#include <tf2_kdl/tf2_kdl.hpp>
#include <kdl_parser/kdl_parser.hpp>
class Pose2Joint: public rclcpp::Node
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)
{
- 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));
+ auto robotDescriptionSubscriber_=create_subscription<std_msgs::msg::String>("robot_description",qos,std::bind(&Pose2Joint::robotDescriptionCB,this,std::placeholders::_1));
while(robotDescription_.empty())
{
RCLCPP_WARN_STREAM_SKIPFIRST_THROTTLE(get_logger(),*get_clock(),1000,"Waiting for robot model on /robot_description.");
q_.resize(chain_.getNrOfJoints());
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));
+ poseSub_=create_subscription<geometry_msgs::msg::PoseStamped>("/pose",10,std::bind(&Pose2Joint::poseCB,this,std::placeholders::_1));
}
void Pose2Joint::poseCB(const geometry_msgs::msg::PoseStamped::SharedPtr poseStamped)
PoseStampedToTf::PoseStampedToTf(const char *node_name,const char *child_frame_id): Node(node_name)
{
child_frame_id_=child_frame_id;
- using std::placeholders::_1;
- poseSubscriber_=create_subscription<geometry_msgs::msg::PoseStamped>("pose",10,std::bind(&PoseStampedToTf::poseStampedCB,this,_1));
+ poseSubscriber_=create_subscription<geometry_msgs::msg::PoseStamped>("pose",10,std::bind(&PoseStampedToTf::poseStampedCB,this,std::placeholders::_1));
tfPublisher_=create_publisher<tf2_msgs::msg::TFMessage>("/tf",10);
}