32 #include<tf/transform_listener.h>
33 #include <nav_msgs/Odometry.h>
34 #include <sensor_msgs/Imu.h>
35 #include "geometry_msgs/Pose.h"
36 #include "geometry_msgs/Quaternion.h"
37 #include "tf/transform_datatypes.h"
39 #include <tf/transform_broadcaster.h>
44 #define PFLN {printf("%s %d\n",__FILE__, __LINE__);}
57 int main(
int argc,
char** argv)
60 ros::init(argc, argv,
"robot_pose2odometry");
62 odom_pub = n.advertise<nav_msgs::Odometry>(
"/robot_pose_odometry", 50);
63 ros::Subscriber imu= n.subscribe(
"/atc/imu/data",1,
ImuCallback);
64 tf::TransformListener listener;
66 tf::TransformBroadcaster odom_broadcaster;
73 bool have_transform=
false;
74 tf::StampedTransform transform_robot_pose_ekf;
77 listener.lookupTransform(
"/world",
"/base_footprint" , ros::Time(0), transform_robot_pose_ekf);
80 catch (tf::TransformException ex)
82 if(listener.waitForTransform(
"/world",
"/base_footprint",ros::Time(0), ros::Duration(3.0)))
86 listener.lookupTransform(
"/world",
"/base_footprint" , ros::Time(0), transform_robot_pose_ekf);
89 catch (tf::TransformException ex)
91 ROS_ERROR(
"Could not lookup transform after waiting 3 secs\n.%s",ex.what());
96 ROS_ERROR(
"Could find valid transform after waiting 3 secs\n.%s",ex.what());
100 if (have_transform==
false)
147 double roll_1,pitch_1,yaw_1;
148 tf::Quaternion q1 = transform_robot_pose_ekf.getRotation();
149 tf::Matrix3x3(q1).getRPY(roll_1, pitch_1, yaw_1);
150 static tf::TransformBroadcaster br;
151 tf::Transform transform;
152 transform.setOrigin( tf::Vector3((transform_robot_pose_ekf.getOrigin()).x(), (transform_robot_pose_ekf.getOrigin()).y(), 0) );
153 transform.setRotation(tf::createQuaternionFromRPY(0.0,0.0, yaw_1) );
155 br.sendTransform(tf::StampedTransform(transform, transform_robot_pose_ekf.stamp_,
"/world",
"/ground" ));
tf::TransformBroadcaster * p_odom_broadcaster
tf::TransformListener * p_listener
sensor_msgs::Imu imu_data
void ImuCallback(const sensor_msgs::Imu::ConstPtr &msg)
int main(int argc, char **argv)
nav_msgs::Odometry odometry_