马上要打Robomaster了,导航决策层想提前取好点设置目标点时出现了个bug,临近比赛时间不够,找了整整一夜,终于找到解决办法 附c++,python代码实现
发布单点导航时,move_base会出现丢失第一次的情况 具体自己可以做一个count计数,一直连续发送目标点,你会发现,count = 2 时 seq才等于1(也就是收到第一个目标点),这也是为什么写的时候连续发move_base又能收到,单发时又收不到的原因 真是令人头大
那么你是不是就自然而然的想要先初始化发布一个目标点,然后后面发送单个的时候就可以成功了,那么你就大错特错了,最核心的bug就是,你需要使用一个c++ ros::Duraton(0.1).sleep(); 或 或者py time.sleep(1) 也就是延时一段时间再去进入你的主循环 我猜测大概率是让move_base内部做一个初始化工作,才能获取到的bug
详见代码 亲测可用
#include <ros/ros.h>
#include <geometry_msgs/PoseStamped.h>
uint32_t count = 0;
uint8_t set_once = 0;
int main(int argc, char **argv) {
// 初始化ROS节点
ros::init(argc, argv, "goal_publisher");
// 创建节点句柄
ros::NodeHandle nh;
// 创建一个Publisher,用于发布目标点消息到/move_base_simple/goal话题
ros::Publisher goal_pub = nh.advertise<geometry_msgs::PoseStamped>("/move_base_simple/goal", 10);
geometry_msgs::PoseStamped goal;
goal.header.seq = 0;
goal.header.stamp = ros::Time(); // 设置当前时间为时间戳
goal.header.frame_id = "robot_foot_init";
goal.pose.position.x = 0;
goal.pose.position.y = 0;
goal.pose.position.z = 0;
goal.pose.orientation.x = 0;
goal.pose.orientation.y = 0;
goal.pose.orientation.z = 0;
goal.pose.orientation.w = 0;
goal_pub.publish(goal);
ros::Duration(0.1).sleep();//bug 初始化时发布一次延时一会后 后面发布的才能正常
// ros::Rate rate(1); // 设置为1Hz,即每秒发布一次目标点
while (ros::ok()) {
// 创建一个PoseStamped消息
// goal.header.stamp = ros::Time::now(); // 设置时间戳为当前时间
if(set_once == 0)
{
// 设置目标点的位置和姿态
goal.header.frame_id = "robot_foot_init"; // 设置坐标系为map
goal.header.stamp = ros::Time::now(); // 设置时间戳为当前时间
goal.pose.position.x = 1.0;
goal.pose.position.y = 3.0;
goal.pose.position.z = 0.0; // 设置为地面
goal.pose.orientation.x = 0.0;
goal.pose.orientation.y = 0.0;
goal.pose.orientation.z = 0.021647965630357723;
goal.pose.orientation.w = 0.9997656553333221;
// 发布目标点消息
goal_pub.publish(goal);
set_once = 1;
}
// rate.sleep();
ros::spinOnce();
}
return 0;
}
#!/usr/bin/python3
# coding=utf8
import rospy
import time
from geometry_msgs.msg import PointStamped, PoseStamped
def send_goal(x,y):
rospy.init_node('send_goal_node',anonymous=True)
pub = rospy.Publisher('/move_base_simple/goal',PoseStamped,queue_size= 10)
goal = PoseStamped()
pub.publish(goal)
time.sleep(1)
goal = PoseStamped()
goal.header.stamp = rospy.Time.now()
goal.header.frame_id = "robot_foot_init"
goal.pose.position.x = x
goal.pose.position.y = y
pub.publish(goal)
if __name__ == '__main__':
send_goal(1.0,2.0)
rospy.spin()