added option to select odom navmsgs topic name

This commit is contained in:
Tan Pin Da
2020-09-07 13:43:05 +08:00
parent 291f2baa08
commit 932b05cf72
4 changed files with 8 additions and 3 deletions

View File

@@ -27,6 +27,8 @@ int main(int argc, char **argv) {
private_node.param<bool>("simulated_robot", messenger.simulated_robot_,
false);
private_node.param<int>("control_rate", messenger.sim_control_rate_, 50);
private_node.param<std::string>("odom_topic_name", messenger.odom_topic_name_,
std::string("odom"));
if (!messenger.simulated_robot_) {
// connect to robot and setup ROS subscription

View File

@@ -24,7 +24,7 @@ ScoutROSMessenger::ScoutROSMessenger(ScoutBase *scout, ros::NodeHandle *nh)
void ScoutROSMessenger::SetupSubscription() {
// odometry publisher
odom_publisher_ = nh_->advertise<nav_msgs::Odometry>(odom_frame_, 50);
odom_publisher_ = nh_->advertise<nav_msgs::Odometry>(odom_topic_name_, 50);
status_publisher_ =
nh_->advertise<scout_msgs::ScoutStatus>("/scout_status", 10);
@@ -239,7 +239,7 @@ void ScoutROSMessenger::PublishOdometryToROS(double linear, double angular,
// publish odometry and tf messages
nav_msgs::Odometry odom_msg;
odom_msg.header.stamp = current_time_;
odom_msg.header.frame_id = odom_frame_;
odom_msg.header.frame_id = "testes";
odom_msg.child_frame_id = base_frame_;
odom_msg.pose.pose.position.x = position_x_;