kdl_conversions
kdl_parser
roscpp
- std_msgs
trajectory_msgs
)
## Generate added messages and services with any dependencies listed here
# generate_messages(
# DEPENDENCIES
-# geometry_msgs# std_msgs# trajectory_msgs
+# geometry_msgs# trajectory_msgs
# )
################################################
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
)
## 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
${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}
-)
#############
<build_depend>kdl_parser</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>
<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>std_msgs</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>std_msgs</exec_depend>
<exec_depend>trajectory_msgs</exec_depend>
+++ /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
-/******************************************************************************
- 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