From c3f596cf89c28cd2c8e3d7e8ab1ac3e02d7c22a5 Mon Sep 17 00:00:00 2001 From: HiepLM Date: Wed, 28 Jan 2026 10:54:18 +0700 Subject: [PATCH] =?UTF-8?q?update=20thuat=20to=C3=A1n=20moi?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../include/amr_control/amr_subcriber.h | 3 + .../Packages/amr_control/src/amr_publiser.cpp | 109 +++++++++--------- .../amr_control/src/amr_subcriber.cpp | 29 ++++- .../Packages/amr_startup/rviz/navigation.rviz | 14 +-- pnkx_nav_core | 2 +- 5 files changed, 93 insertions(+), 64 deletions(-) diff --git a/Controllers/Packages/amr_control/include/amr_control/amr_subcriber.h b/Controllers/Packages/amr_control/include/amr_control/amr_subcriber.h index a707f76..51d85f7 100644 --- a/Controllers/Packages/amr_control/include/amr_control/amr_subcriber.h +++ b/Controllers/Packages/amr_control/include/amr_control/amr_subcriber.h @@ -4,6 +4,7 @@ #include #include #include +#include #include #include #include @@ -18,9 +19,11 @@ namespace amr_control private: void moveBaseSimpleCallback(const geometry_msgs::PoseStamped::ConstPtr &msg); + void odometryCallback(const nav_msgs::Odometry::ConstPtr &msg); ros::NodeHandle nh_; ros::Subscriber move_base_simple_sub_; + ros::Subscriber odometry_sub_; robot::move_base_core::BaseNavigation::Ptr move_base_ptr_; }; } // namespace amr_control diff --git a/Controllers/Packages/amr_control/src/amr_publiser.cpp b/Controllers/Packages/amr_control/src/amr_publiser.cpp index 7d5bf54..b7269ab 100644 --- a/Controllers/Packages/amr_control/src/amr_publiser.cpp +++ b/Controllers/Packages/amr_control/src/amr_publiser.cpp @@ -338,60 +338,6 @@ void AmrPublisher::onNewSubscriptionLocal(const ros::SingleSubscriberPublisher& publishLocalCostmap(); } -void AmrPublisher::convertToNavPath(const robot_nav_2d_msgs::Path2D &robot_path, - nav_msgs::Path &nav_path) -{ - // Convert header - nav_2d_msgs::Path2D path2d; - path2d.header.seq = robot_path.header.seq; - path2d.header.frame_id = robot_path.header.frame_id; - path2d.header.stamp = ros::Time::now(); - path2d.poses.resize(robot_path.poses.size()); - for (unsigned int i = 0; i < robot_path.poses.size(); i++) - { - path2d.poses[i].pose.x = robot_path.poses[i].pose.x; - path2d.poses[i].pose.y = robot_path.poses[i].pose.y; - path2d.poses[i].pose.theta = robot_path.poses[i].pose.theta; - } - nav_path = nav_2d_utils::pathToPath(path2d); -} - -void AmrPublisher::publishGlobalPlan() -{ - if(!move_base_ptr_) - { - return; - } - - // Check if plan is empty - if(cached_global_plan_.poses.empty()) - { - return; - } - - robot::Time now = robot::Time::now(); - robot::Time plan_stamp = cached_global_plan_.header.stamp; - - // Check if timestamp is zero (uninitialized) - if(plan_stamp.sec == 0 && plan_stamp.nsec == 0) - { - return; - } - - robot::Duration age = now - plan_stamp; - robot::Duration max_age(0.5); - - // Check if timestamp is not older than 0.5 seconds - if(plan_stamp < now - max_age) - { - return; - } - - nav_msgs::Path nav_path; - convertToNavPath(cached_global_plan_, nav_path); - - global_planner_obj_.plan_pub_.publish(nav_path); -} void AmrPublisher::onNewSubscriptionGlobal(const ros::SingleSubscriberPublisher& pub) { @@ -465,6 +411,61 @@ void AmrPublisher::cmdVelTimerCallback(const ros::WallTimerEvent &event) } } +void AmrPublisher::convertToNavPath(const robot_nav_2d_msgs::Path2D &robot_path, + nav_msgs::Path &nav_path) +{ + // Convert header + nav_2d_msgs::Path2D path2d; + path2d.header.seq = robot_path.header.seq; + path2d.header.frame_id = robot_path.header.frame_id; + path2d.header.stamp = ros::Time::now(); + path2d.poses.resize(robot_path.poses.size()); + for (unsigned int i = 0; i < robot_path.poses.size(); i++) + { + path2d.poses[i].pose.x = robot_path.poses[i].pose.x; + path2d.poses[i].pose.y = robot_path.poses[i].pose.y; + path2d.poses[i].pose.theta = robot_path.poses[i].pose.theta; + } + nav_path = nav_2d_utils::pathToPath(path2d); +} + +void AmrPublisher::publishGlobalPlan() +{ + if(!move_base_ptr_) + { + return; + } + + // Check if plan is empty + if(cached_global_plan_.poses.empty()) + { + return; + } + + robot::Time now = robot::Time::now(); + robot::Time plan_stamp = cached_global_plan_.header.stamp; + + // Check if timestamp is zero (uninitialized) + if(plan_stamp.sec == 0 && plan_stamp.nsec == 0) + { + return; + } + + robot::Duration age = now - plan_stamp; + robot::Duration max_age(0.5); + + // Check if timestamp is not older than 0.5 seconds + if(plan_stamp < now - max_age) + { + return; + } + + nav_msgs::Path nav_path; + convertToNavPath(cached_global_plan_, nav_path); + + global_planner_obj_.plan_pub_.publish(nav_path); +} + void AmrPublisher::publishLocalPlan() { if(!move_base_ptr_) diff --git a/Controllers/Packages/amr_control/src/amr_subcriber.cpp b/Controllers/Packages/amr_control/src/amr_subcriber.cpp index b2681cb..7cb1b75 100644 --- a/Controllers/Packages/amr_control/src/amr_subcriber.cpp +++ b/Controllers/Packages/amr_control/src/amr_subcriber.cpp @@ -5,8 +5,10 @@ amr_control::AmrSubscriber::AmrSubscriber(ros::NodeHandle &nh, robot::move_base_ : nh_(nh), move_base_ptr_(move_base_ptr) { robot::log_info("[%s:%d] Initializing AmrSubscriber namespace: %s", __FILE__, __LINE__, nh_.getNamespace().c_str()); - ros::NodeHandle nh_move_base_simple; - move_base_simple_sub_ = nh_move_base_simple.subscribe("/move_base_simple/goal", 1, &AmrSubscriber::moveBaseSimpleCallback, this); + ros::NodeHandle nh_move_base; + move_base_simple_sub_ = nh_move_base.subscribe("/move_base_simple/goal", 1, &AmrSubscriber::moveBaseSimpleCallback, this); + odometry_sub_ = nh_move_base.subscribe("/odom", 1, &AmrSubscriber::odometryCallback, this); + } amr_control::AmrSubscriber::~AmrSubscriber() @@ -25,4 +27,27 @@ void amr_control::AmrSubscriber::moveBaseSimpleCallback(const geometry_msgs::Pos goal.pose.orientation.z = msg->pose.orientation.z; goal.pose.orientation.w = msg->pose.orientation.w; move_base_ptr_->moveTo(goal, 0.02, 0.02); +} + +void amr_control::AmrSubscriber::odometryCallback(const nav_msgs::Odometry::ConstPtr &msg) +{ + if (!move_base_ptr_) + { + return; + } + robot_nav_msgs::Odometry odometry; + odometry.header.stamp = robot::Time::now(); + odometry.header.frame_id = msg->header.frame_id; + odometry.child_frame_id = msg->child_frame_id; + odometry.pose.pose.position.x = msg->pose.pose.position.x; + odometry.pose.pose.position.y = msg->pose.pose.position.y; + odometry.pose.pose.position.z = msg->pose.pose.position.z; + odometry.pose.pose.orientation.x = msg->pose.pose.orientation.x; + odometry.pose.pose.orientation.y = msg->pose.pose.orientation.y; + odometry.pose.pose.orientation.z = msg->pose.pose.orientation.z; + odometry.pose.pose.orientation.w = msg->pose.pose.orientation.w; + odometry.twist.twist.linear.x = msg->twist.twist.linear.x; + odometry.twist.twist.linear.y = msg->twist.twist.linear.y; + odometry.twist.twist.angular.z = msg->twist.twist.angular.z; + move_base_ptr_->addOdometry("odometry", odometry); } \ No newline at end of file diff --git a/Controllers/Packages/amr_startup/rviz/navigation.rviz b/Controllers/Packages/amr_startup/rviz/navigation.rviz index 986e7a0..2fdfb5d 100644 --- a/Controllers/Packages/amr_startup/rviz/navigation.rviz +++ b/Controllers/Packages/amr_startup/rviz/navigation.rviz @@ -7,8 +7,8 @@ Panels: - /Global Options1 - /Grid1/Offset1 - /TF1/Frames1 - - /Global Map1 - /Local Map1 + - /Local Map1/Plan1 Splitter Ratio: 0.37295082211494446 Tree Height: 682 - Class: rviz/Selection @@ -420,7 +420,7 @@ Visualization Manager: - Alpha: 1 Buffer Length: 1 Class: rviz/Path - Color: 138; 226; 52 + Color: 239; 41; 41 Enabled: true Head Diameter: 0.30000001192092896 Head Length: 0.20000000298023224 @@ -431,7 +431,7 @@ Visualization Manager: Offset: X: 0 Y: 0 - Z: 0 + Z: 0.6000000238418579 Pose Color: 255; 85; 255 Pose Style: Axes Queue Size: 10 @@ -604,10 +604,10 @@ Visualization Manager: Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Scale: -105.0660171508789 + Scale: -191.32327270507812 Target Frame: base_link - X: 2.555877447128296 - Y: 1.0760011672973633 + X: 0.8287617564201355 + Y: 0.9812669157981873 Saved: - Angle: -34.55989074707031 Class: rviz/TopDownOrtho @@ -629,7 +629,7 @@ Window Geometry: Height: 833 Hide Left Dock: false Hide Right Dock: false - QMainWindow State: 000000ff00000000fd000000040000000000000156000002e7fc0200000008fb000000100044006900730070006c006100790073000000003d000002e7000000c900fffffffb0000001200530065006c0065006300740069006f006e000000003d000002590000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001b500000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb0000000a0049006d006100670065000000003d0000039e0000000000000000fb0000000c00430061006d006500720061020000007c0000013600000280000001e0fb0000000c00430061006d0065007200610200000644000000b7000002c1000002f9000000010000010f00000259fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d00000259000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b20000000000000000000000020000041800000168fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007520000003efc0100000002fb0000000800540069006d00650000000000000007520000041800fffffffb0000000800540069006d0065010000000000000450000000000000000000000435000002e700000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd0000000400000000000001fa000002e7fc0200000008fb000000100044006900730070006c006100790073010000003d000002e7000000c900fffffffb0000001200530065006c0065006300740069006f006e000000003d000002590000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001b500000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb0000000a0049006d006100670065000000003d0000039e0000000000000000fb0000000c00430061006d006500720061020000007c0000013600000280000001e0fb0000000c00430061006d0065007200610200000644000000b7000002c1000002f9000000010000010f00000259fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d00000259000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b20000000000000000000000020000041800000168fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007520000003efc0100000002fb0000000800540069006d00650000000000000007520000041800fffffffb0000000800540069006d0065010000000000000450000000000000000000000235000002e700000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Time: diff --git a/pnkx_nav_core b/pnkx_nav_core index 0f58db3..8578985 160000 --- a/pnkx_nav_core +++ b/pnkx_nav_core @@ -1 +1 @@ -Subproject commit 0f58db34810790067177408f37dc0becb91620c1 +Subproject commit 85789855a80bb93e682bff8a9c1b102f129a4621