赞
踩
打字不易,转载请注明。
use_landmarks= true
此时命令行窗口显示
此时程序会挂起,一直等到接收到landmark话题才开始运行。因此landmark话题要一直按一定频率(10HZ)发送,没有观测到landmark时要发送空的话题。
首先通过ros topic list查看此时cartographer发布的话题
rostopic list
找到所需要发布的landmark话题 其中还有/landmark_poses_list话题为cartographer接收到/landmark信息后发出的landmark的坐标
查看其数据类型
mini@mini-IRU:~/catkin_ws_universal$ rostopic type /landmark
cartographer_ros_msgs/LandmarkList
再查看该消息的组成
即对应的在msg文件中写的格式。
所有我们要定义发布一个该消息类型的话题。
首先 创建一个工作空间,并按照cartographer中的msg定义同样的msg消息类型
再编写landmark话题发布程序。
在cartographer中,对于数据数组大小没有要求,因此就随便定义数量为10的数组
#include "ros/ros.h" #include "pub_landmark/LandmarkList.h" #include "pub_landmark/LandmarkEntry.h" #include "geometry_msgs/Pose.h" #include "std_msgs/String.h" #include "std_msgs/Header.h" int main(int argc, char *argv[]) { ros::init(argc, argv, "pub_landmark"); ros::NodeHandle nh; ros::NodeHandle n("~"); ros::Publisher landmark_pub = n.advertise<pub_landmark::LandmarkList>("landmark", 10); pub_landmark::LandmarkList landmark_0; ros::Rate loop_rate(10); int count=0; while (ros::ok()) { landmark_0.header.stamp = ros::Time::now(); landmark_0.header.frame_id = "base_link"; landmark_0.landmarks.resize(10); for(int t=0;t<10;t++) { std::stringstream ss; ss << t;//"landmark_" ;//<< count/ << "_" t; landmark_0.landmarks[t].id = ss.str(); landmark_0.landmarks[t].tracking_from_landmark_transform.position.x = 1.0*t; landmark_0.landmarks[t].tracking_from_landmark_transform.position.y = 2.0*t; landmark_0.landmarks[t].tracking_from_landmark_transform.position.z = 3.0*t; landmark_0.landmarks[t].tracking_from_landmark_transform.orientation.w = 1.0*t; landmark_0.landmarks[t].tracking_from_landmark_transform.orientation.x = 0.0*t; landmark_0.landmarks[t].tracking_from_landmark_transform.orientation.y = 0.0*t; landmark_0.landmarks[t].tracking_from_landmark_transform.orientation.z = 0.0*t; landmark_0.landmarks[t].translation_weight = 1.0*t; landmark_0.landmarks[t].rotation_weight = 2.0*t; } landmark_pub.publish(landmark_0); ROS_INFO("%d", count); loop_rate.sleep(); count++; } return 0; }
运行此节点后将发布自己创建的landmark话题。
若此时cartographer与发布landmark的节点一起运行,会出现这种报错:
此问题是由两个工作空间中的话题的MD5sum码不同导致的。如果按照cartographer这边的码来,则可以找到目录
这其中有cartographer定义的所有msg的头文件,打开landmarklist.h
找到
复制该码到pub_landmark发布节点的包中
找到代码中同样的位置,替换上md5sum码再进行编译就可以了.
(关于这个md5码的问题,只要两个包的码一样就可以,因此将pub_lanmark中的码复制到cartographer的包里也是可以的。若修改pub_landmark包,在通过rostopic查看pub_landmark发布的landmark话题时会导致错误(通过rostopic命令发布好像也用到了该码))
由此,cartographer便能接收到landmark话题数据,并且在rviz中显示出来了
(图中给了固定的十个点)
可以查看cartographer接收到landmark话题消息后发布的landmrk_pose_list
其中id包括0-9.
Copyright © 2003-2013 www.wpsshop.cn 版权所有,并保留所有权利。