feat: add feedback

master
朱雅鸿 2024-07-22 10:13:01 +08:00
parent 8d89011cdf
commit 26c96bf055
3 changed files with 43 additions and 2 deletions

View File

@ -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 包装器获取)

View File

@ -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

View File

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