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;
-    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);
 }