关于rospy和roscpp订阅中的spin
区别
使用后发现rospy的订阅者不需rospy.spin()就可以进行订阅信息并调用回调函数。而roscpp则必须要有ros::spin()或者 ros::spinOnce()才会调用。
对于rospy,把发送端频率设为10,即1秒发送10次,接受端的测试代码:
点击查看代码
import rospy
import time
from std_msgs.msg import String
def callback(data):
rospy.loginfo(rospy.get_caller_id() + "I heard %s", data.data)
def listener():
# In ROS, nodes are uniquely named. If two nodes with the same
# name are launched, the previous one is kicked off. The
# anonymous=True flag means that rospy will choose a unique
# name for our 'listener' node so that multiple listeners can
# run simultaneously.
rospy.init_node('listener', anonymous=True)
rate = rospy.Rate(20)
print("before subscriber.")
rospy.Subscriber("chatter", String, callback)
print("after subscriber.")
# while not rospy.is_shutdown():
# print("not shutdown.")
# rate.sleep()
# print("outside")
# rospy.spin()
# print("outside")
time.sleep(0.2)
# spin() simply keeps python from exiting until this node is stopped
if __name__ == '__main__':
listener()
点击查看代码
hqtech@hqtech-ROG-Zephyrus-M16:~/zzy_ws$ rosrun driver listener.py
before subscriber.
after subscriber.
[INFO] [1662099023.089818]: /listener_29781_1662099022854I heard hello world 1662099023.0857892
[INFO] [1662099023.189953]: /listener_29781_1662099022854I heard hello world 1662099023.185761
hqtech@hqtech-ROG-Zephyrus-M16:~/zzy_ws$
time.sleep(0.2)恰好接收到两次。由此可见,rospy的订阅端并不会由rospy.spin()控制是否执行,而仅由程序阻塞,或者说运行的时间控制。回调执行时机只能说在rospy.Subscriber("chatter", String, callback)之后,只要程序还没结束并且收到消息就会执行。我猜测底层应该是新开了一个线程,但我们不能控制它执行的时机。官网的注释写的是很准确的。
关于ros::spin()和 ros::spinOnce()
对于ros::spin()比较直接,就是程序运行到此处直接阻塞,Ctrl + C才会结束,执行时机是运行到ros::spin()时。而对于ros::spinOnce()运行时机和ros::spin()一样,但他记录的是执行 ros::Subscriber sub = n.subscribe("chatter", 1000, chatterCallback);之后到ros::spinOnce()之间的时间所收到消息,执行对应次数的回调函数。通过控制这段时间,或者循环和rate.sleep();可以达到我们想要的效果。使用比较灵活。
接收端测试代码:
点击查看代码
#include "ros/ros.h"
#include "std_msgs/String.h"
#include <unistd.h>
void chatterCallback(const std_msgs::String::ConstPtr& msg) {
ROS_INFO("I heard: [%s]", msg->data.c_str());
}
int main(int argc, char **argv) {
ros::init(argc, argv, "listener");
ros::NodeHandle n;
ros::Subscriber sub = n.subscribe("chatter", 1000, chatterCallback);
ros::Rate loop_rate(20);
sleep(static_cast<unsigned int>(1));
ros::spinOnce();
printf("after spin.\n");
// while (ros::ok()) {
// printf("before spin");
// sleep(1);
// ros::spinOnce();
// ros::spin();
// printf("after spin\n");
// loop_rate.sleep();
// }
return 0;
}
输出:
点击查看代码
hqtech@hqtech-ROG-Zephyrus-M16:~/zzy_ws$ rosrun driver listener
[ INFO] [1662100104.940729044]: I heard: [hello world 1662100104.2338932]
[ INFO] [1662100104.942940732]: I heard: [hello world 1662100104.3338606]
[ INFO] [1662100104.942992571]: I heard: [hello world 1662100104.4338796]
[ INFO] [1662100104.943017042]: I heard: [hello world 1662100104.5338557]
[ INFO] [1662100104.943038588]: I heard: [hello world 1662100104.633874]
[ INFO] [1662100104.943063684]: I heard: [hello world 1662100104.7338858]
[ INFO] [1662100104.943087774]: I heard: [hello world 1662100104.8338528]
[ INFO] [1662100104.943117424]: I heard: [hello world 1662100104.9339075]
after spin.
hqtech@hqtech-ROG-Zephyrus-M16:~/zzy_ws$

浙公网安备 33010602011771号