diff --git a/launch/gazebo.launch b/launch/gazebo.launch new file mode 100644 index 0000000000000000000000000000000000000000..9b2ca58690ccb7254d28187b278d82ee4fe00ca1 --- /dev/null +++ b/launch/gazebo.launch @@ -0,0 +1,4 @@ +<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 diff --git a/src/navsat_to_odom.cpp b/src/navsat_to_odom.cpp index 456cffcbaa393c8cd77108e86f19b7b66fb33591..182fda513b24d61b6e8dcc7ed96eb761cfdb9c6b 100644 --- a/src/navsat_to_odom.cpp +++ b/src/navsat_to_odom.cpp @@ -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_; };