feat: add feedback

mpc_local_planner/include/mpc_local_planner/mpc_local_planner_ros.h

@@ -52,6 +52,7 @@
 // transforms
 #include <tf2/utils.h>
 #include <tf2_ros/buffer.h>
+#include <tf/transform_listener.h>
 
 // costmap
 #include <costmap_2d/costmap_2d_ros.h>
@@ -374,6 +375,12 @@ class MpcLocalPlannerROS : public nav_core::BaseLocalPlanner, public mbf_costmap
  private:
     // Definition of member variables
 
+    tf::TransformListener listener;
+    ros::Publisher state_pub;
+    ros::Timer timer;
+
+    void timerCallback(const ros::TimerEvent&);
+
     // 外部对象（存储弱指针）
     costmap_2d::Costmap2DROS* _costmap_ros;  //!< 指向从导航栈接收的代价地图 ros 包装器的指针
     costmap_2d::Costmap2D* _costmap;         //!< 指向 2D 代价地图的指针（从代价地图 ros 包装器获取）

mpc_local_planner/scripts/plot_optimal_control_results.py

@@ -125,10 +125,10 @@ if __name__ == '__main__':
    
     rospy.init_node("ocp_result_plotter", anonymous=True) 
 
-    topic_name = "/test_mpc_optim_node/ocp_result"
+    topic_name = "/move_base/MpcLocalPlannerROS/ocp_result"
     topic_name = rospy.get_param('~ocp_result_topic', topic_name)
 
-    plot_states = rospy.get_param('~plot_states', False)
+    plot_states = rospy.get_param('~plot_states', True)
 
     result_plotter = OcpResultPlotter(plot_states, topic_name)
     rate = 2

mpc_local_planner/src/mpc_local_planner_ros.cpp

@@ -111,6 +111,36 @@ void MpcLocalPlannerROS::reconfigureCollisionCB(CollisionReconfigureConfig& conf
     _params.collision_check_no_poses               = config.collision_check_no_poses;
 }
 
+
+void MpcLocalPlannerROS::timerCallback(const ros::TimerEvent&)
+{
+    tf::StampedTransform transform;
+    try
+    {
+        // 监听最近的变换
+        listener.lookupTransform("map", "base_link", ros::Time(0), transform);
+        
+        // 提取x, y和yaw
+        double x = transform.getOrigin().x();
+        double y = transform.getOrigin().y();
+        double yaw = tf::getYaw(transform.getRotation());
+
+        // 创建和发布状态消息
+        mpc_local_planner_msgs::StateFeedback state_msg;
+        state_msg.header.stamp = ros::Time::now();
+        state_msg.header.frame_id = "map";
+        state_msg.state.push_back(x);
+        state_msg.state.push_back(y);
+        state_msg.state.push_back(yaw);
+        state_pub.publish(state_msg);
+    }
+    catch (tf::TransformException &ex)
+    {
+        ROS_ERROR("%s", ex.what());
+    }
+}
+
+
 /**
  * @brief 初始化局部规划器
  *
@@ -128,6 +158,10 @@ void MpcLocalPlannerROS::initialize(std::string name, tf2_ros::Buffer* tf, costm
         // 创建一个Node Handle，使用插件的名称（如在move_base中加载）
         ros::NodeHandle nh("~/" + name);
 
+        // 初始化发布器和定时器
+        state_pub = nh.advertise<mpc_local_planner_msgs::StateFeedback>("state_feedback", 10);
+        timer = nh.createTimer(ros::Duration(0.1), &MpcLocalPlannerROS::timerCallback, this); // 每0.1秒执行一次
+
         // 加载插件相关的主要参数
         nh.param("controller/xy_goal_tolerance", _params.xy_goal_tolerance, _params.xy_goal_tolerance);
         nh.param("controller/yaw_goal_tolerance", _params.yaw_goal_tolerance, _params.yaw_goal_tolerance);