diff --git a/agv_pro_base/package.xml b/agv_pro_base/package.xml index c669a21..4193ea4 100644 --- a/agv_pro_base/package.xml +++ b/agv_pro_base/package.xml @@ -2,7 +2,7 @@ agv_pro_base - 1.0.1 + 1.0.2 Control Nodes for AGV Pro lanni BSD-3-Clause license diff --git a/agv_pro_base/src/agv_pro_ros.cpp b/agv_pro_base/src/agv_pro_ros.cpp index f22e75b..b8af605 100644 --- a/agv_pro_base/src/agv_pro_ros.cpp +++ b/agv_pro_base/src/agv_pro_ros.cpp @@ -196,15 +196,8 @@ void AGV_PRO::publisherVoltage() } void AGV_PRO::publisherOdom(double dt) -{ - geometry_msgs::msg::TransformStamped odom_trans; - odom_trans.header.stamp = this->get_clock()->now(); - odom_trans.header.frame_id = "odom"; - odom_trans.child_frame_id = "base_footprint"; - - tf2::Quaternion quat; - quat.setRPY(0.0, 0.0, theta); - geometry_msgs::msg::Quaternion odom_quat = tf2::toMsg(quat); +{ + currentTime = this->get_clock()->now(); double delta_x = (vx * cos(theta) - vy * sin(theta)) * dt; double delta_y = (vx * sin(theta) + vy * cos(theta)) * dt; @@ -214,18 +207,26 @@ void AGV_PRO::publisherOdom(double dt) y += delta_y; theta += delta_th; + geometry_msgs::msg::TransformStamped odom_trans; + odom_trans.header.stamp = currentTime; + odom_trans.header.frame_id = frame_id_of_odometry_; + odom_trans.child_frame_id = child_frame_id_of_odometry_; + + tf2::Quaternion quat; + quat.setRPY(0.0, 0.0, theta); + geometry_msgs::msg::Quaternion odom_quat = tf2::toMsg(quat); + odom_trans.transform.translation.x = x; odom_trans.transform.translation.y = y; odom_trans.transform.translation.z = 0.0; - odom_trans.transform.rotation = odom_quat; odomBroadcaster->sendTransform(odom_trans); nav_msgs::msg::Odometry odom; - odom.header.stamp = this->get_clock()->now();; - odom.header.frame_id = "odom"; - odom.child_frame_id = "base_footprint"; + odom.header.stamp = currentTime; + odom.header.frame_id = frame_id_of_odometry_; + odom.child_frame_id = child_frame_id_of_odometry_; odom.pose.pose.position.x = x; odom.pose.pose.position.y = y;