roscpp、rospy 用法汇总和代码实战

roscpp、rospy 用法汇总和代码实战
【ROS】publish发布和subscribe订阅topic话题中的消息message
使用 ZeroMQ 将消息从你的程序发布到 ROS 中并通过 rostopic 查看发布的内容
ros::init ros::NodeHandle ros::Subscriber ros::Publisher他们之间是如何联系和工作的？
如何理解my_map.yaml中origin的含义
遍历激光雷达数据
python代码实现提取ros topic 的position信息并保存到文件
【ROS】创建ROS的消息msg和srv服务项目实战

roscpp 和 rospy 是 ROS (Robot Operating System) 中两个非常重要的包，分别用于 C++ 和 Python 语言环境。它们用于创建和管理 ROS 节点，实现话题的发布/订阅、服务的提供/使用、参数的设置/获取等功能。
下面我将分别为这些功能提供 Python (rospy) 和 C++ (roscpp) 的代码示例。

1. 话题发布（Publisher）和订阅（Subscriber）

Python (rospy)

发布者（Publisher）

import rospy
from std_msgs.msg import String

def talker():
    pub = rospy.Publisher('chatter', String, queue_size=10)
    rospy.init_node('talker', anonymous=True)
    rate = rospy.Rate(10)  # 10hz
    while not rospy.is_shutdown():
        hello_str = "hello world %s" % rospy.get_time()
        rospy.loginfo(hello_str)
        pub.publish(hello_str)
        rate.sleep()

if __name__ == '__main__':
    try:
        talker()
    except rospy.ROSInterruptException:
        pass

订阅者（Subscriber）

import rospy
from std_msgs.msg import String

def callback(data):
    rospy.loginfo(rospy.get_caller_id() + " I heard %s", data.data)

def listener():
    rospy.init_node('listener', anonymous=True)
    rospy.Subscriber("chatter", String, callback)
    rospy.spin()

if __name__ == '__main__':
    listener()

C++ (roscpp)

发布者（Publisher）

#include "ros/ros.h"
#include "std_msgs/String.h"
#include <sstream>

int main(int argc, char **argv) {
    ros::init(argc, argv, "talker");
    ros::NodeHandle n;
    ros::Publisher chatter_pub = n.advertise<std_msgs::String>("chatter", 1000);
    ros::Rate loop_rate(10);

    while (ros::ok()) {
        std_msgs::String msg;
        std::stringstream ss;
        ss << "hello world " << ros::Time::now();
        msg.data = ss.str();
        ROS_INFO("%s", msg.data.c_str());
        chatter_pub.publish(msg);
        ros::spinOnce();
        loop_rate.sleep();
    }
    return 0;
}

订阅者（Subscriber）

#include "ros/ros.h"
#include "std_msgs/String.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::spin();
    return 0;
}

2. 服务（Service）

Python (rospy)

服务端（Server）

import rospy
from std_srvs.srv import Empty, EmptyResponse

def handle_service(req):
    print("Service called")
    return EmptyResponse()

def service_server():
    rospy.init_node('service_server')
    s = rospy.Service('example_service', Empty, handle_service)
    print("Ready to handle requests.")
    rospy.spin()

if __name__ == "__main__":
    service_server()

客户端（Client）

import rospy
from std_srvs.srv import Empty

def call_service():
    rospy.init_node('service_client')
    rospy.wait_for_service('example_service')
    try:
        service = rospy.ServiceProxy('example_service', Empty)
        response = service()
        print("Service executed")
    except rospy.ServiceException as e:
        print("Service call failed: %s" % e)

if __name__ == "__main__":
    call_service()

C++ (roscpp)

服务端（Server）

#include "ros/ros.h"
#include "std_srvs/Empty.h"

bool handle_service(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res) {
    ROS_INFO("Service called");
    return true;
}

int main(int argc, char **argv) {
    ros::init(argc, argv, "service_server");
    ros::NodeHandle n;
    ros::ServiceServer service = n.advertiseService("example_service", handle_service);
    ROS_INFO("Ready to handle requests.");
    ros::spin();
    return 0;
}

客户端（Client）

#include "ros/ros.h"
#include "std_srvs/Empty.h"

int main(int argc, char **argv) {
    ros::init(argc, argv, "service_client");
    ros::NodeHandle n;
    ros::ServiceClient client = n.serviceClient<std_srvs::Empty>("example_service");
    std_srvs::Empty srv;

    if (client.call(srv)) {
        ROS_INFO("Service executed");
    } else {
        ROS_ERROR("Failed to call service");
        return 1;
    }
    return 0;
}

3. 参数设置/获取（Parameter Server）

Python (rospy)

import rospy

rospy.init_node('param_example')

# 设置参数
rospy.set_param('example_param', 'hello world')

# 获取参数
param = rospy.get_param('example_param')
print("Got param: " + str(param))

C++ (roscpp)

#include "ros/ros.h"

int main(int argc, char **argv) {
    ros::init(argc, argv, "param_example");
    ros::NodeHandle n;

    // 设置参数
    n.setParam("example_param", "hello world");

    // 获取参数
    std::string param;
    if (n.getParam("example_param", param)) {
        ROS_INFO("Got param: %s", param.c_str());
    } else {
        ROS_ERROR("Failed to get param 'example_param'");
    }

    return 0;
}

这些示例展示了如何在 ROS 中使用 roscpp 和 rospy 完成基本的机器人编程任务，包括话题的发布和订阅、服务的提供和调用，以及参数服务器的使用。在实际应用中，这些技术经常被用于构建复杂的机器人系统和应用。