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

Add param

parent 757717e2
......@@ -22,8 +22,12 @@ public:
sub_vel_simul_ = n_.subscribe("vel_simul", 10, &NavsatToOdom::velSimulCallback, this);
pub_odom_ = n_.advertise<nav_msgs::Odometry>("odom", 1);
transform_stamped_.header.frame_id = "odom";
transform_stamped_.child_frame_id = "base_footprint";
ros::NodeHandle n_tilde("~");
n_tilde.param("publish_tf", publish_tf_, false);
n_tilde.param<std::string>("frame_parent", frame_parent_, "odom");
n_tilde.param<std::string>("frame_child", frame_child_, "base_footprint");
transform_stamped_.header.frame_id = frame_parent_;
transform_stamped_.child_frame_id = frame_child_;
}
void fixCallback(const sensor_msgs::NavSatFix::ConstPtr & fix)
......@@ -42,17 +46,21 @@ public:
nav_msgs::Odometry odom;
odom.header.stamp = ros::Time::now();
odom.header.frame_id = "odom";
odom.header.frame_id = frame_parent_;
odom.child_frame_id = frame_child_;
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;
transform_stamped_.transform.translation.y = north;
transform_stamped_.transform.rotation = tf::createQuaternionMsgFromYaw(course_);
br_.sendTransform(transform_stamped_);
if(publish_tf_ == true)
{
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_);
}
}
else
{
......@@ -82,6 +90,8 @@ private:
ros::NodeHandle n_;
ros::Subscriber sub_fix_, sub_vel_, sub_vel_simul_;
ros::Publisher pub_odom_;
bool publish_tf_;
std::string frame_parent_, frame_child_;
GeographicLib::LocalCartesian enu_;
bool is_enu_initialized_;
......
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