27 #include <nav_msgs/Odometry.h> 28 #include <geometry_msgs/PoseStamped.h> 29 #include <geometry_msgs/Vector3Stamped.h> 30 #include <geometry_msgs/TransformStamped.h> 31 #include <sensor_msgs/Imu.h> 56 #ifndef TF_MATRIX3x3_H 63 transforms.push_back(geometry_msgs::TransformStamped());
67 void sendTransform(geometry_msgs::Pose
const &pose,
const std_msgs::Header& header, std::string child_frame_id =
"")
69 std::vector<geometry_msgs::TransformStamped> transforms;
79 if (child_frame_id.empty()) child_frame_id =
"base_link";
131 if (g_pose_publisher) {
132 geometry_msgs::PoseStamped pose_stamped;
133 pose_stamped.pose = pose;
134 pose_stamped.header = header;
135 g_pose_publisher.
publish(pose_stamped);
139 if (g_euler_publisher) {
140 geometry_msgs::Vector3Stamped euler_stamped;
141 euler_stamped.vector.x = roll;
142 euler_stamped.vector.y = pitch;
143 euler_stamped.vector.z = yaw;
144 euler_stamped.header = header;
145 g_euler_publisher.
publish(euler_stamped);
150 sendTransform(odometry.pose.pose, odometry.header, odometry.child_frame_id);
158 geometry_msgs::Pose pose;
159 pose.position.x = tf.transform.translation.x;
160 pose.position.y = tf.transform.translation.y;
161 pose.position.z = tf.transform.translation.z;
162 pose.orientation = tf.transform.rotation;
168 std::vector<geometry_msgs::TransformStamped> transforms;
169 std::string child_frame_id;
172 tf.
stamp_ = imu.header.stamp;
176 if (child_frame_id.empty()) child_frame_id =
"base_link";
192 if (!transforms.empty()) g_transform_broadcaster->
sendTransform(transforms);
195 if (g_pose_publisher) {
196 geometry_msgs::PoseStamped pose_stamped;
197 pose_stamped.header.stamp = imu.header.stamp;
200 g_pose_publisher.
publish(pose_stamped);
206 nav_msgs::Odometry::ConstPtr odom = input.
instantiate<nav_msgs::Odometry>();
211 if (input.
getDataType() ==
"geometry_msgs/PoseStamped") {
212 geometry_msgs::PoseStamped::ConstPtr pose = input.
instantiate<geometry_msgs::PoseStamped>();
218 sensor_msgs::Imu::ConstPtr imu = input.
instantiate<sensor_msgs::Imu>();
223 if (input.
getDataType() ==
"geometry_msgs/TransformStamped") {
224 geometry_msgs::TransformStamped::ConstPtr
tf = input.
instantiate<geometry_msgs::TransformStamped>();
229 ROS_ERROR_THROTTLE(1.0,
"message_to_tf received a %s message. Supported message types: nav_msgs/Odometry geometry_msgs/PoseStamped geometry_msgs/TransformStamped sensor_msgs/Imu", input.
getDataType().c_str());
232 int main(
int argc,
char** argv) {
285 if (subscribers == 0) {
286 ROS_FATAL(
"Usage: rosrun message_to_tf message_to_tf <topic>");
288 }
else if (subscribers > 1) {
289 ROS_FATAL(
"More than one of the parameters odometry_topic, pose_topic, imu_topic and topic are set.\n" 290 "Please specify exactly one of them or simply add the topic name to the command line.");
294 bool publish_pose =
true;
295 priv_nh.
getParam(
"publish_pose", publish_pose);
297 std::string publish_pose_topic;
298 priv_nh.
getParam(
"publish_pose_topic", publish_pose_topic);
300 if (!publish_pose_topic.empty())
301 g_pose_publisher = node.
advertise<geometry_msgs::PoseStamped>(publish_pose_topic, 10);
303 g_pose_publisher = priv_nh.
advertise<geometry_msgs::PoseStamped>(
"pose", 10);
306 bool publish_euler =
true;
307 priv_nh.
getParam(
"publish_euler", publish_euler);
309 std::string publish_euler_topic;
310 priv_nh.
getParam(
"publish_euler_topic", publish_euler_topic);
312 if (!publish_euler_topic.empty())
313 g_euler_publisher = node.
advertise<geometry_msgs::Vector3Stamped>(publish_euler_topic, 10);
315 g_euler_publisher = priv_nh.
advertise<geometry_msgs::Vector3Stamped>(
"euler", 10);
static tf::Quaternion createQuaternionFromRPY(double roll, double pitch, double yaw)
bool g_publish_roll_pitch
ros::Publisher g_euler_publisher
std::string g_position_frame_id
std::string g_odometry_topic
static void pointMsgToTF(const geometry_msgs::Point &msg_v, Point &bt_v)
std::string g_child_frame_id
void publish(const boost::shared_ptr< M > &message) const
std::string getPrefixParam(ros::NodeHandle &nh)
TFSIMD_FORCE_INLINE void setX(tfScalar x)
TFSIMD_FORCE_INLINE void setY(tfScalar y)
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
TFSIMD_FORCE_INLINE void setZ(tfScalar z)
void tfCallback(geometry_msgs::TransformStamped const &tf)
void odomCallback(nav_msgs::Odometry const &odometry)
ROSCPP_DECL void spin(Spinner &spinner)
void imuCallback(sensor_msgs::Imu const &imu)
std::string resolve(const std::string &prefix, const std::string &frame_name)
TFSIMD_FORCE_INLINE const tfScalar & x() const
#define ROS_ERROR_THROTTLE(rate,...)
TFSIMD_FORCE_INLINE const tfScalar & z() const
static void quaternionMsgToTF(const geometry_msgs::Quaternion &msg, Quaternion &bt)
static void quaternionTFToMsg(const Quaternion &bt, geometry_msgs::Quaternion &msg)
TFSIMD_FORCE_INLINE const tfScalar & y() const
void sendTransform(geometry_msgs::Pose const &pose, const std_msgs::Header &header, std::string child_frame_id="")
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
void poseCallback(geometry_msgs::PoseStamped const &pose)
std::string g_stabilized_frame_id
void addTransform(std::vector< geometry_msgs::TransformStamped > &transforms, const tf::StampedTransform &tf)
tf::TransformBroadcaster * g_transform_broadcaster
void multiCallback(topic_tools::ShapeShifter const &input)
int main(int argc, char **argv)
ros::Publisher g_pose_publisher
bool getParam(const std::string &key, std::string &s) const
std::string g_footprint_frame_id
static void transformStampedTFToMsg(const StampedTransform &bt, geometry_msgs::TransformStamped &msg)
static const Matrix3x3 & getIdentity()