Commit 508a5c4c authored by Rousseau Vincent's avatar Rousseau Vincent
Browse files

Put back odom publisher

parent 36edd13e
......@@ -12,26 +12,6 @@ find_package(catkin REQUIRED COMPONENTS ${${PROJECT_NAME}_CATKIN_DEPS})
## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
# catkin_python_setup()
################################################
## Declare ROS dynamic reconfigure parameters ##
################################################
## To declare and build dynamic reconfigure parameters within this
## package, follow these steps:
## * In the file package.xml:
## * add a build_depend and a run_depend tag for "dynamic_reconfigure"
## * In this file (CMakeLists.txt):
## * add "dynamic_reconfigure" to
## find_package(catkin REQUIRED COMPONENTS ...)
## * uncomment the "generate_dynamic_reconfigure_options" section below
## and list every .cfg file to be processed
## Generate dynamic reconfigure parameters in the 'cfg' folder
# generate_dynamic_reconfigure_options(
# cfg/DynReconf1.cfg
# cfg/DynReconf2.cfg
# )
###################################
## catkin specific configuration ##
###################################
......
......@@ -20,6 +20,7 @@ public:
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";
......@@ -37,6 +38,14 @@ 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);
transform_stamped_.header.stamp = ros::Time::now();
transform_stamped_.transform.translation.x = east;
......@@ -68,9 +77,10 @@ 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_;
bool is_enu_initialized_;
double course_;
tf2_ros::TransformBroadcaster br_;
......@@ -86,4 +96,4 @@ int main(int argc, char** argv)
ROS_INFO_STREAM("navsat_to_odom");
ros::spin();
return 0;
}
\ No newline at end of file
}
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