From: Walter Fetter Lages Date: Thu, 6 May 2021 09:21:16 +0000 (-0300) Subject: Remove joint_demux.cpp and pose_stamped2tf.cpp. Use topic_tools package transform... X-Git-Url: http://git.ece.ufrgs.br/?a=commitdiff_plain;h=c7cafee5674be4acaed917215044b2e22cc3244d;p=trajectory_conversions.git Remove joint_demux.cpp and pose_stamped2tf.cpp. Use topic_tools package transform for the same functionality. --- diff --git a/CMakeLists.txt b/CMakeLists.txt index a26fe96..9a9c2b1 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -12,7 +12,6 @@ find_package(catkin REQUIRED COMPONENTS kdl_conversions kdl_parser roscpp - std_msgs trajectory_msgs ) @@ -74,7 +73,7 @@ find_package(orocos_kdl REQUIRED) ## Generate added messages and services with any dependencies listed here # generate_messages( # DEPENDENCIES -# geometry_msgs# std_msgs# trajectory_msgs +# geometry_msgs# trajectory_msgs # ) ################################################ @@ -109,7 +108,7 @@ find_package(orocos_kdl REQUIRED) catkin_package( # INCLUDE_DIRS include # LIBRARIES trajectory_conversions -# CATKIN_DEPENDS geometry_msgs kdl_conversions kdl_parser roscpp std_msgs trajectory_msgs +# CATKIN_DEPENDS geometry_msgs kdl_conversions kdl_parser roscpp trajectory_msgs # DEPENDS orocos_kdl ) @@ -140,8 +139,6 @@ include_directories( ## 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 @@ -158,14 +155,6 @@ target_link_libraries(pose_stamped2joint ${catkin_LIBRARIES} ${orocos_kdl_LIBRARIES} ) -target_link_libraries(pose_stamped2tf - ${catkin_LIBRARIES} - ${orocos_kdl_LIBRARIES} -) -target_link_libraries(joint_demux - ${catkin_LIBRARIES} - ${orocos_kdl_LIBRARIES} -) ############# diff --git a/package.xml b/package.xml index e0ff130..774666d 100644 --- a/package.xml +++ b/package.xml @@ -55,21 +55,18 @@ kdl_parser orocos_kdl roscpp - std_msgs trajectory_msgs geometry_msgs kdl_conversions kdl_parser orocos_kdl roscpp - std_msgs trajectory_msgs geometry_msgs kdl_conversions kdl_parser orocos_kdl roscpp - std_msgs trajectory_msgs diff --git a/src/joint_demux.cpp b/src/joint_demux.cpp deleted file mode 100644 index d7a64ec..0000000 --- a/src/joint_demux.cpp +++ /dev/null @@ -1,88 +0,0 @@ -/* - 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_stamped2tf.cpp b/src/pose_stamped2tf.cpp deleted file mode 100644 index 461d32d..0000000 --- a/src/pose_stamped2tf.cpp +++ /dev/null @@ -1,86 +0,0 @@ -/****************************************************************************** - 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; -}