update thuat toán moi

Controllers/Packages/amr_control/include/amr_control/amr_subcriber.h

@@ -4,6 +4,7 @@
 #include <ros/ros.h>
 #include <robot/robot.h>
 #include <geometry_msgs/PoseStamped.h>
+#include <nav_msgs/Odometry.h>
 #include <move_base_core/navigation.h>
 #include <memory>
 #include <string>
@@ -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

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_)

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;
+    ros::NodeHandle nh_move_base;
-    move_base_simple_sub_ = nh_move_base_simple.subscribe("/move_base_simple/goal", 1, &AmrSubscriber::moveBaseSimpleCallback, this);
+    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);
 }

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
+      X: 0.8287617564201355
-      Y: 1.0760011672973633
+      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:

pnkx_nav_core

@@ -1 +1 @@
-Subproject commit 0f58db34810790067177408f37dc0becb91620c1
+Subproject commit 85789855a80bb93e682bff8a9c1b102f129a4621