Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Port to tf2 and enable using static broadcaster #28

Merged
merged 1 commit into from
Oct 16, 2015
Merged

Port to tf2 and enable using static broadcaster #28

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
4 changes: 2 additions & 2 deletions CMakeLists.txt
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -3,14 +3,14 @@ project(robot_state_publisher)

find_package(orocos_kdl REQUIRED)
find_package(catkin REQUIRED
COMPONENTS roscpp rosconsole rostime tf tf_conversions kdl_parser cmake_modules
COMPONENTS roscpp rosconsole rostime tf tf2_ros tf2_kdl kdl_parser cmake_modules
)
find_package(Eigen REQUIRED)

catkin_package(
LIBRARIES ${PROJECT_NAME}_solver
INCLUDE_DIRS include
DEPENDS roscpp rosconsole rostime tf tf_conversions kdl_parser orocos_kdl
DEPENDS roscpp rosconsole rostime tf2_ros tf2_kdl kdl_parser orocos_kdl
)

include_directories(SYSTEM ${EIGEN_INCLUDE_DIRS})
Expand Down
1 change: 1 addition & 0 deletions include/robot_state_publisher/joint_state_listener.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -74,6 +74,7 @@ class JointStateListener{
ros::Time last_callback_time_;
std::map<std::string, ros::Time> last_publish_time_;
MimicMap mimic_;
bool use_tf_static_;

};
}
Expand Down
16 changes: 11 additions & 5 deletions include/robot_state_publisher/robot_state_publisher.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -40,8 +40,9 @@
#include <ros/ros.h>
#include <boost/scoped_ptr.hpp>
#include <tf/tf.h>
#include <tf/transform_broadcaster.h>
#include <urdf/model.h>
#include <tf2_ros/static_transform_broadcaster.h>
#include <tf2_ros/transform_broadcaster.h>
#include <kdl/frames.hpp>
#include <kdl/segment.hpp>
#include <kdl/tree.hpp>
Expand All @@ -63,27 +64,32 @@ class RobotStatePublisher
{
public:
/** Constructor
* \param tree The kinematic model of a robot, represented by a KDL Tree
* \param tree The kinematic model of a robot, represented by a KDL Tree
*/
RobotStatePublisher(const KDL::Tree& tree, const urdf::Model& model = urdf::Model());

/// Destructor
~RobotStatePublisher(){};

/** Publish transforms to tf
* \param joint_positions A map of joint names and joint positions.
/** Publish transforms to tf
* \param joint_positions A map of joint names and joint positions.
* \param time The time at which the joint positions were recorded
*/
void publishTransforms(const std::map<std::string, double>& joint_positions, const ros::Time& time, const std::string& tf_prefix);
void publishFixedTransforms(const std::string& tf_prefix);
void publishFixedTransforms(const std::string& tf_prefix, bool use_tf_static);

private:
void addChildren(const KDL::SegmentMap::const_iterator segment);


std::map<std::string, SegmentPair> segments_, segments_fixed_;
<<<<<<< HEAD
Copy link

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Yup. Sure enough, the merge conflict is here. Good catch, @remod. Did this branch need to be rebased before merging?

Copy link

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

This is fixed by #36

tf::TransformBroadcaster tf_broadcaster_;
const urdf::Model& model_;
=======
tf2_ros::TransformBroadcaster tf_broadcaster_;
tf2_ros::StaticTransformBroadcaster static_tf_broadcaster_;
>>>>>>> Port to tf2 and enable using static broadcaster
};


Expand Down
8 changes: 4 additions & 4 deletions include/robot_state_publisher/treefksolverposfull_recursive.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -24,7 +24,7 @@
#define KDLTREEFKSOLVERPOSFULL_RECURSIVE_HPP

#include <kdl/tree.hpp>
#include <tf/transform_datatypes.h>
#include <tf2/transform_datatypes.h>


namespace KDL {
Expand All @@ -36,12 +36,12 @@ class TreeFkSolverPosFull_recursive
TreeFkSolverPosFull_recursive(const Tree& _tree);
~TreeFkSolverPosFull_recursive();

int JntToCart(const std::map<std::string, double>& q_in, std::map<std::string, tf::Stamped<Frame> >& p_out, bool flatten_tree=true);
int JntToCart(const std::map<std::string, double>& q_in, std::map<std::string, tf2::Stamped<Frame> >& p_out, bool flatten_tree=true);

private:
void addFrameToMap(const std::map<std::string, double>& q_in,
std::map<std::string, tf::Stamped<KDL::Frame> >& p_out,
const tf::Stamped<KDL::Frame>& previous_frame,
std::map<std::string, tf2::Stamped<KDL::Frame> >& p_out,
const tf2::Stamped<KDL::Frame>& previous_frame,
const SegmentMap::const_iterator this_segment,
bool flatten_tree);

Expand Down
6 changes: 4 additions & 2 deletions package.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -28,7 +28,8 @@
<build_depend>rostime</build_depend>
<build_depend>sensor_msgs</build_depend>
<build_depend>tf</build_depend>
<build_depend>tf_conversions</build_depend>
<build_depend>tf2_ros</build_depend>
<build_depend>tf2_kdl</build_depend>
<build_depend>cmake_modules</build_depend>

<run_depend>catkin</run_depend>
Expand All @@ -40,5 +41,6 @@
<run_depend>rostime</run_depend>
<run_depend>sensor_msgs</run_depend>
<run_depend>tf</run_depend>
<run_depend>tf_conversions</run_depend>
<run_depend>tf2_ros</run_depend>
<run_depend>tf2_kdl</run_depend>
</package>
7 changes: 5 additions & 2 deletions src/joint_state_listener.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -56,6 +56,8 @@ JointStateListener::JointStateListener(const KDL::Tree& tree, const MimicMap& m,
// set publish frequency
double publish_freq;
n_tilde.param("publish_frequency", publish_freq, 50.0);
// set whether to use the /tf_static latched static transform broadcaster
n_tilde.param("use_tf_static", use_tf_static_, false);
// get the tf_prefix parameter from the closest namespace
std::string tf_prefix_key;
n_tilde.searchParam("tf_prefix", tf_prefix_key);
Expand All @@ -66,7 +68,8 @@ JointStateListener::JointStateListener(const KDL::Tree& tree, const MimicMap& m,
joint_state_sub_ = n.subscribe("joint_states", 1, &JointStateListener::callbackJointState, this);

// trigger to publish fixed joints
timer_ = n_tilde.createTimer(publish_interval_, &JointStateListener::callbackFixedJoint, this);
// if using static transform broadcaster, this will be a oneshot trigger and only run once
timer_ = n_tilde.createTimer(publish_interval_, &JointStateListener::callbackFixedJoint, this, use_tf_static_);

};

Expand All @@ -77,7 +80,7 @@ JointStateListener::~JointStateListener()

void JointStateListener::callbackFixedJoint(const ros::TimerEvent& e)
{
state_publisher_.publishFixedTransforms(tf_prefix_);
state_publisher_.publishFixedTransforms(tf_prefix_, use_tf_static_);
}

void JointStateListener::callbackJointState(const JointStateConstPtr& state)
Expand Down
47 changes: 24 additions & 23 deletions src/robot_state_publisher.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,13 +1,13 @@
/*********************************************************************
* Software License Agreement (BSD License)
*
*
* Copyright (c) 2008, Willow Garage, Inc.
* All rights reserved.
*
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
Expand All @@ -17,7 +17,7 @@
* * Neither the name of the Willow Garage nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
Expand All @@ -36,7 +36,9 @@

#include "robot_state_publisher/robot_state_publisher.h"
#include <kdl/frames_io.hpp>
#include <tf_conversions/tf_kdl.h>
#include <geometry_msgs/TransformStamped.h>
#include <tf2_kdl/tf2_kdl.h>


using namespace std;
using namespace ros;
Expand Down Expand Up @@ -84,43 +86,42 @@ namespace robot_state_publisher{
void RobotStatePublisher::publishTransforms(const map<string, double>& joint_positions, const Time& time, const std::string& tf_prefix)
{
ROS_DEBUG("Publishing transforms for moving joints");
std::vector<tf::StampedTransform> tf_transforms;
tf::StampedTransform tf_transform;
tf_transform.stamp_ = time;
std::vector<geometry_msgs::TransformStamped> tf_transforms;

// loop over all joints
for (map<string, double>::const_iterator jnt=joint_positions.begin(); jnt != joint_positions.end(); jnt++){
std::map<std::string, SegmentPair>::const_iterator seg = segments_.find(jnt->first);
if (seg != segments_.end()){
tf::transformKDLToTF(seg->second.segment.pose(jnt->second), tf_transform);
tf_transform.frame_id_ = tf::resolve(tf_prefix, seg->second.root);
tf_transform.child_frame_id_ = tf::resolve(tf_prefix, seg->second.tip);
geometry_msgs::TransformStamped tf_transform = tf2::kdlToTransform(seg->second.segment.pose(jnt->second));
tf_transform.header.stamp = time;
tf_transform.header.frame_id = tf::resolve(tf_prefix, seg->second.root);
tf_transform.child_frame_id = tf::resolve(tf_prefix, seg->second.tip);
tf_transforms.push_back(tf_transform);
}
}
tf_broadcaster_.sendTransform(tf_transforms);
}


// publish fixed transforms
void RobotStatePublisher::publishFixedTransforms(const std::string& tf_prefix)
void RobotStatePublisher::publishFixedTransforms(const std::string& tf_prefix, bool use_tf_static)
{
ROS_DEBUG("Publishing transforms for fixed joints");
std::vector<tf::StampedTransform> tf_transforms;
tf::StampedTransform tf_transform;
tf_transform.stamp_ = ros::Time::now()+ros::Duration(0.5); // future publish by 0.5 seconds
std::vector<geometry_msgs::TransformStamped> tf_transforms;
geometry_msgs::TransformStamped tf_transform;

// loop over all fixed segments
for (map<string, SegmentPair>::const_iterator seg=segments_fixed_.begin(); seg != segments_fixed_.end(); seg++){
tf::transformKDLToTF(seg->second.segment.pose(0), tf_transform);
tf_transform.frame_id_ = tf::resolve(tf_prefix, seg->second.root);
tf_transform.child_frame_id_ = tf::resolve(tf_prefix, seg->second.tip);
geometry_msgs::TransformStamped tf_transform = tf2::kdlToTransform(seg->second.segment.pose(0));
tf_transform.header.stamp = ros::Time::now();
tf_transform.header.frame_id = tf::resolve(tf_prefix, seg->second.root);
tf_transform.child_frame_id = tf::resolve(tf_prefix, seg->second.tip);
tf_transforms.push_back(tf_transform);
}
tf_broadcaster_.sendTransform(tf_transforms);
if(use_tf_static){
static_tf_broadcaster_.sendTransform(tf_transforms);
}else{
tf_broadcaster_.sendTransform(tf_transforms);
}
}

}



16 changes: 8 additions & 8 deletions src/treefksolverposfull_recursive.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -39,12 +39,12 @@ TreeFkSolverPosFull_recursive::~TreeFkSolverPosFull_recursive()
}


int TreeFkSolverPosFull_recursive::JntToCart(const map<string, double>& q_in, map<string, tf::Stamped<Frame> >& p_out, bool flatten_tree)
int TreeFkSolverPosFull_recursive::JntToCart(const map<string, double>& q_in, map<string, tf2::Stamped<Frame> >& p_out, bool flatten_tree)
{
// clear output
p_out.clear();

addFrameToMap(q_in, p_out, tf::Stamped<KDL::Frame>(KDL::Frame::Identity(), ros::Time(), GetTreeElementSegment(tree.getRootSegment()->second).getName()),
addFrameToMap(q_in, p_out, tf2::Stamped<KDL::Frame>(KDL::Frame::Identity(), ros::Time(), GetTreeElementSegment(tree.getRootSegment()->second).getName()),
tree.getRootSegment(), flatten_tree);

return 0;
Expand All @@ -53,13 +53,13 @@ int TreeFkSolverPosFull_recursive::JntToCart(const map<string, double>& q_in, ma


void TreeFkSolverPosFull_recursive::addFrameToMap(const map<string, double>& q_in,
map<string, tf::Stamped<Frame> >& p_out,
const tf::Stamped<KDL::Frame>& previous_frame,
map<string, tf2::Stamped<Frame> >& p_out,
const tf2::Stamped<KDL::Frame>& previous_frame,
const SegmentMap::const_iterator this_segment,
bool flatten_tree)
{
// get pose of this segment
tf::Stamped<KDL::Frame> this_frame;
tf2::Stamped<KDL::Frame> this_frame;
double jnt_p = 0;
if (GetTreeElementSegment(this_segment->second).getJoint().getType() != Joint::None){
map<string, double>::const_iterator jnt_pos = q_in.find(GetTreeElementSegment(this_segment->second).getJoint().getName());
Expand All @@ -69,18 +69,18 @@ void TreeFkSolverPosFull_recursive::addFrameToMap(const map<string, double>& q_i
}
jnt_p = jnt_pos->second;
}
this_frame = tf::Stamped<KDL::Frame>(previous_frame * GetTreeElementSegment(this_segment->second).pose(jnt_p), ros::Time(), previous_frame.frame_id_);
this_frame = tf2::Stamped<KDL::Frame>(previous_frame * GetTreeElementSegment(this_segment->second).pose(jnt_p), ros::Time(), previous_frame.frame_id_);

if (this_segment->first != tree.getRootSegment()->first)
p_out.insert(make_pair(this_segment->first, tf::Stamped<KDL::Frame>(this_frame, ros::Time(), previous_frame.frame_id_)));
p_out.insert(make_pair(this_segment->first, tf2::Stamped<KDL::Frame>(this_frame, ros::Time(), previous_frame.frame_id_)));

// get poses of child segments
for (vector<SegmentMap::const_iterator>::const_iterator child = GetTreeElementChildren(this_segment->second).begin();
child != GetTreeElementChildren(this_segment->second).end(); child++){
if (flatten_tree)
addFrameToMap(q_in, p_out, this_frame, *child, flatten_tree);
else
addFrameToMap(q_in, p_out, tf::Stamped<KDL::Frame>(KDL::Frame::Identity(), ros::Time(), this_segment->first), *child, flatten_tree);
addFrameToMap(q_in, p_out, tf2::Stamped<KDL::Frame>(KDL::Frame::Identity(), ros::Time(), this_segment->first), *child, flatten_tree);
}
}

Expand Down