Commit a74fb212 authored by Rousseau Vincent's avatar Rousseau Vincent
Browse files

Add tf broadcaster

parent 889cd5f3
cmake_minimum_required(VERSION 2.8.3)
project(navsat_to_odom)
set(${PROJECT_NAME}_CATKIN_DEPS roscpp sensor_msgs nav_msgs)
set(${PROJECT_NAME}_CATKIN_DEPS roscpp sensor_msgs nav_msgs tf2_ros tf2_geometry_msgs)
## Find catkin macros and libraries
## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
......
......@@ -16,6 +16,8 @@
<depend>sensor_msgs</depend>
<depend>nav_msgs</depend>
<depend>geographiclib</depend>
<depend>tf2_ros</depend>
<depend>tf2_geometry_msgs</depend>
<!-- Use test_depend for packages you need only for testing: -->
<!-- <test_depend>gtest</test_depend> -->
......
#include <ros/ros.h>
#include <sensor_msgs/NavSatFix.h>
#include <geometry_msgs/TwistStamped.h>
#include <nav_msgs/Odometry.h>
#include <tf/transform_datatypes.h>
#include <tf2_ros/transform_listener.h>
#include <tf2_ros/transform_broadcaster.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <geometry_msgs/TransformStamped.h>
#include <GeographicLib/LocalCartesian.hpp>
......@@ -12,15 +15,16 @@ public:
NavsatToOdom(ros::NodeHandle n) :
n_(n),
is_enu_initialized_(false),
course_(0)
course_(0),
tf_listener_(tf_buffer_)
{
sub_fix_ = n_.subscribe("navsat/fix", 10, &NavsatToOdom::fixCallback, this);
sub_vel_ = n_.subscribe("navsat/vel", 10, &NavsatToOdom::velCallback, this);
sub_vel_simul_ = n_.subscribe("navsat/vel_simul", 10, &NavsatToOdom::velSimulCallback, this);
pub_odom_ = n_.advertise<nav_msgs::Odometry>("navsat/odom", 1);
/* tf2_ros::Buffer tfBuffer;
tf2_ros::TransformListener tfListener(tfBuffer); */
transform_stamped_.header.frame_id = "odom";
transform_stamped_.child_frame_id = "base_footprint";
}
void fixCallback(const sensor_msgs::NavSatFix::ConstPtr & fix)
......@@ -41,10 +45,26 @@ public:
odom.header.frame_id = "odom";
odom.pose.pose.position.x = east;
odom.pose.pose.position.y = north;
odom.pose.pose.orientation = tf::createQuaternionMsgFromYaw(course_);
pub_odom_.publish(odom);
/* try{
transform_stamped_ = tf_buffer_.lookupTransform("navsat_link", "base_footprint", ros::Time(0));
}
catch (tf2::TransformException &ex) {
ROS_WARN("%s",ex.what());
}
geometry_msgs::Pose pose_out;
tf2::doTransform(odom.pose.pose, pose_out, transform_stamped_);
ROS_INFO_STREAM("pose out x "<< pose_out.position.x<< " y "<< pose_out.position.y); */
transform_stamped_.header.stamp = ros::Time::now();
transform_stamped_.transform.translation.x = east;
transform_stamped_.transform.translation.y = north;
transform_stamped_.transform.rotation = tf::createQuaternionMsgFromYaw(course_);
br_.sendTransform(transform_stamped_);
}
}
......@@ -74,6 +94,11 @@ private:
GeographicLib::LocalCartesian enu_;
std::atomic<bool> is_enu_initialized_;
double course_;
tf2_ros::Buffer tf_buffer_;
tf2_ros::TransformListener tf_listener_;
tf2_ros::TransformBroadcaster br_;
geometry_msgs::TransformStamped transform_stamped_;
};
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment