Commit 36edd13e authored by Rousseau Vincent's avatar Rousseau Vincent
Browse files

Clean and add launch file

parent a74fb212
<launch>
<include file="$(find sensors_gazebo)/launch/boxes.launch" />
<node name="navsat_to_odom" pkg="navsat_to_odom" type="navsat_to_odom_node" output="screen" />
</launch>
\ No newline at end of file
......@@ -15,13 +15,11 @@ public:
NavsatToOdom(ros::NodeHandle n) :
n_(n),
is_enu_initialized_(false),
course_(0),
tf_listener_(tf_buffer_)
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);
transform_stamped_.header.frame_id = "odom";
transform_stamped_.child_frame_id = "base_footprint";
......@@ -39,25 +37,6 @@ public:
}
double east, north, up;
enu_.Forward(fix->latitude, fix->longitude, 0.0, east, north, up);
nav_msgs::Odometry odom;
odom.header.stamp = ros::Time::now();
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;
......@@ -89,14 +68,11 @@ public:
private:
ros::NodeHandle n_;
ros::Subscriber sub_fix_, sub_vel_, sub_vel_simul_;
ros::Publisher pub_odom_;
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