2 #include <nav_msgs/Odometry.h> 3 #include <geometry_msgs/PoseStamped.h> 4 #include <geometry_msgs/Vector3Stamped.h> 5 #include <geometry_msgs/TransformStamped.h> 6 #include <sensor_msgs/Imu.h> 31 #ifndef TF_MATRIX3x3_H 38 transforms.push_back(geometry_msgs::TransformStamped());
42 void sendTransform(geometry_msgs::Pose
const &pose,
const std_msgs::Header& header, std::string child_frame_id =
"")
44 std::vector<geometry_msgs::TransformStamped> transforms;
54 if (child_frame_id.empty()) child_frame_id =
"base_link";
106 if (g_pose_publisher) {
107 geometry_msgs::PoseStamped pose_stamped;
108 pose_stamped.pose = pose;
109 pose_stamped.header = header;
110 g_pose_publisher.
publish(pose_stamped);
114 if (g_euler_publisher) {
115 geometry_msgs::Vector3Stamped euler_stamped;
116 euler_stamped.vector.x = roll;
117 euler_stamped.vector.y = pitch;
118 euler_stamped.vector.z = yaw;
119 euler_stamped.header = header;
120 g_euler_publisher.
publish(euler_stamped);
125 sendTransform(odometry.pose.pose, odometry.header, odometry.child_frame_id);
133 geometry_msgs::Pose pose;
134 pose.position.x = tf.transform.translation.x;
135 pose.position.y = tf.transform.translation.y;
136 pose.position.z = tf.transform.translation.z;
137 pose.orientation = tf.transform.rotation;
143 std::vector<geometry_msgs::TransformStamped> transforms;
144 std::string child_frame_id;
147 tf.
stamp_ = imu.header.stamp;
151 if (child_frame_id.empty()) child_frame_id =
"base_link";
167 if (!transforms.empty()) g_transform_broadcaster->
sendTransform(transforms);
170 if (g_pose_publisher) {
171 geometry_msgs::PoseStamped pose_stamped;
172 pose_stamped.header.stamp = imu.header.stamp;
175 g_pose_publisher.
publish(pose_stamped);
181 nav_msgs::Odometry::ConstPtr odom = input.
instantiate<nav_msgs::Odometry>();
186 if (input.
getDataType() ==
"geometry_msgs/PoseStamped") {
187 geometry_msgs::PoseStamped::ConstPtr pose = input.
instantiate<geometry_msgs::PoseStamped>();
193 sensor_msgs::Imu::ConstPtr imu = input.
instantiate<sensor_msgs::Imu>();
198 if (input.
getDataType() ==
"geometry_msgs/TransformStamped") {
199 geometry_msgs::TransformStamped::ConstPtr
tf = input.
instantiate<geometry_msgs::TransformStamped>();
204 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());
207 int main(
int argc,
char** argv) {
260 if (subscribers == 0) {
261 ROS_FATAL(
"Usage: rosrun message_to_tf message_to_tf <topic>");
263 }
else if (subscribers > 1) {
264 ROS_FATAL(
"More than one of the parameters odometry_topic, pose_topic, imu_topic and topic are set.\n" 265 "Please specify exactly one of them or simply add the topic name to the command line.");
269 bool publish_pose =
true;
270 priv_nh.
getParam(
"publish_pose", publish_pose);
272 std::string publish_pose_topic;
273 priv_nh.
getParam(
"publish_pose_topic", publish_pose_topic);
275 if (!publish_pose_topic.empty())
276 g_pose_publisher = node.
advertise<geometry_msgs::PoseStamped>(publish_pose_topic, 10);
278 g_pose_publisher = priv_nh.
advertise<geometry_msgs::PoseStamped>(
"pose", 10);
281 bool publish_euler =
true;
282 priv_nh.
getParam(
"publish_euler", publish_euler);
284 std::string publish_euler_topic;
285 priv_nh.
getParam(
"publish_euler_topic", publish_euler_topic);
287 if (!publish_euler_topic.empty())
288 g_euler_publisher = node.
advertise<geometry_msgs::Vector3Stamped>(publish_euler_topic, 10);
290 g_euler_publisher = priv_nh.
advertise<geometry_msgs::Vector3Stamped>(
"euler", 10);
int main(int argc, char **argv)
static tf::Quaternion createQuaternionFromRPY(double roll, double pitch, double yaw)
static void pointMsgToTF(const geometry_msgs::Point &msg_v, Point &bt_v)
void publish(const boost::shared_ptr< M > &message) const
std::string getPrefixParam(ros::NodeHandle &nh)
TFSIMD_FORCE_INLINE void setX(tfScalar x)
std::string g_stabilized_frame_id
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())
std::string g_child_frame_id
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
TFSIMD_FORCE_INLINE void setZ(tfScalar z)
tf::TransformBroadcaster * g_transform_broadcaster
void imuCallback(sensor_msgs::Imu const &imu)
ros::Publisher g_pose_publisher
ROSCPP_DECL void spin(Spinner &spinner)
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)
void tfCallback(geometry_msgs::TransformStamped const &tf)
static void quaternionTFToMsg(const Quaternion &bt, geometry_msgs::Quaternion &msg)
TFSIMD_FORCE_INLINE const tfScalar & y() const
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
std::string g_footprint_frame_id
bool g_publish_roll_pitch
void sendTransform(geometry_msgs::Pose const &pose, const std_msgs::Header &header, std::string child_frame_id="")
bool getParam(const std::string &key, std::string &s) const
ros::Publisher g_euler_publisher
void addTransform(std::vector< geometry_msgs::TransformStamped > &transforms, const tf::StampedTransform &tf)
void poseCallback(geometry_msgs::PoseStamped const &pose)
static void transformStampedTFToMsg(const StampedTransform &bt, geometry_msgs::TransformStamped &msg)
std::string g_position_frame_id
static const Matrix3x3 & getIdentity()
void multiCallback(topic_tools::ShapeShifter const &input)
void odomCallback(nav_msgs::Odometry const &odometry)
std::string g_odometry_topic