Commit 889cd5f3 authored by Rousseau Vincent's avatar Rousseau Vincent
Browse files

Adding orientation to odom

parent 4cc655a6
......@@ -3,6 +3,7 @@
#include <sensor_msgs/NavSatFix.h>
#include <geometry_msgs/TwistStamped.h>
#include <nav_msgs/Odometry.h>
#include <tf/transform_datatypes.h>
#include <GeographicLib/LocalCartesian.hpp>
......@@ -10,11 +11,16 @@ class NavsatToOdom {
public:
NavsatToOdom(ros::NodeHandle n) :
n_(n),
is_enu_initialized_(false)
is_enu_initialized_(false),
course_(0)
{
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); */
}
void fixCallback(const sensor_msgs::NavSatFix::ConstPtr & fix)
......@@ -36,22 +42,38 @@ public:
odom.pose.pose.position.x = east;
odom.pose.pose.position.y = north;
odom.pose.pose.orientation = tf::createQuaternionMsgFromYaw(course_);
pub_odom_.publish(odom);
}
}
void velCallback(const geometry_msgs::TwistStamped::ConstPtr & fix)
void velCallback(const geometry_msgs::TwistStamped::ConstPtr & vel)
{
double speed = std::sqrt(std::pow(vel->twist.linear.x,2) + std::pow(vel->twist.linear.y,2));
if(speed > 0.2)
{
course_ = atan2(vel->twist.linear.y, vel->twist.linear.x) + M_PI_2;
}
}
void velSimulCallback(const geometry_msgs::Vector3Stamped::ConstPtr & vel)
{
double speed = std::sqrt(std::pow(vel->vector.x,2) + std::pow(vel->vector.y,2));
if(speed > 0.2)
{
course_ = atan2(vel->vector.y, vel->vector.x) + M_PI_2;
}
}
private:
ros::NodeHandle n_;
ros::Subscriber sub_fix_, sub_vel_;
ros::Subscriber sub_fix_, sub_vel_, sub_vel_simul_;
ros::Publisher pub_odom_;
GeographicLib::LocalCartesian enu_;
std::atomic<bool> is_enu_initialized_;
double course_;
};
......
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