第一章 ROS基础

#!/usr/bin/env python
import rospy

def main():
    # 初始化节点
    rospy.init_node('my_node', anonymous=True)
    rospy.loginfo("节点启动成功")

    # 保持节点运行
    rospy.spin()

if __name__ == '__main__':
    main()

#!/usr/bin/env python
import rospy
from std_msgs.msg import String

def talker():
    rospy.init_node('talker', anonymous=True)
    pub = rospy.Publisher('chatter', String, queue_size=10)
    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

#!/usr/bin/env python
import rospy
from std_msgs.msg import String

def callback(data):
    rospy.loginfo("接收到数据: %s", data.data)

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

if __name__ == '__main__':
    listener()

#!/usr/bin/env python
from rospy_tutorials.srv import AddTwoInts, AddTwoIntsResponse
import rospy

def handle_add_two_ints(req):
    result = req.a + req.b
    rospy.loginfo("返回 [%s + %s = %s]" % (req.a, req.b, result))
    return AddTwoIntsResponse(result)

def add_two_ints_server():
    rospy.init_node('add_two_ints_server')
    s = rospy.Service('add_two_ints', AddTwoInts, handle_add_two_ints)
    rospy.loginfo("准备添加两个整数")
    rospy.spin()

if __name__ == "__main__":
    add_two_ints_server()

#!/usr/bin/env python
import sys
import rospy
from rospy_tutorials.srv import AddTwoInts

def add_two_ints_client(x, y):
    rospy.wait_for_service('add_two_ints')
    try:
        add_two_ints = rospy.ServiceProxy('add_two_ints', AddTwoInts)
        resp = add_two_ints(x, y)
        return resp.sum
    except rospy.ServiceException as e:
        print("服务调用失败: %s" % e)

if __name__ == "__main__":
    if len(sys.argv) == 3:
        x = int(sys.argv[1])
        y = int(sys.argv[2])
    else:
        x, y = 1, 2

    result = add_two_ints_client(x, y)
    print("%s + %s = %s" % (x, y, result))

#!/usr/bin/env python
import rospy
import actionlib
from actionlib_tutorials.msg import FibonacciAction, FibonacciFeedback, FibonacciResult

class FibonacciActionServer:
    def __init__(self):
        self.server = actionlib.SimpleActionServer(
            'fibonacci', FibonacciAction, self.execute, False)
        self.server.start()

    def execute(self, goal):
        r = rospy.Rate(1)
        feedback = FibonacciFeedback()
        result = FibonacciResult()

        feedback.sequence = [0, 1]

        for i in range(1, goal.order):
            if self.server.is_preempt_requested():
                self.server.set_preempted()
                return

            feedback.sequence.append(
                feedback.sequence[i] + feedback.sequence[i-1])
            self.server.publish_feedback(feedback)
            r.sleep()

        result.sequence = feedback.sequence
        self.server.set_succeeded(result)

if __name__ == '__main__':
    rospy.init_node('fibonacci_server')
    server = FibonacciActionServer()
    rospy.spin()

#!/usr/bin/env python
import rospy

rospy.init_node('param_demo')

# 设置参数
rospy.set_param('robot_name', 'my_robot')
rospy.set_param('max_speed', 1.5)

# 获取参数
robot_name = rospy.get_param('robot_name', 'default_robot')
max_speed = rospy.get_param('max_speed', 1.0)

# 检查参数是否存在
if rospy.has_param('robot_name'):
    rospy.loginfo("参数存在: %s", robot_name)

# 删除参数
rospy.delete_param('robot_name')

#!/usr/bin/env python
import rospy
from my_robot.msg import CustomMsg

def publisher():
    rospy.init_node('custom_pub')
    pub = rospy.Publisher('custom_topic', CustomMsg, queue_size=10)
    rate = rospy.Rate(10)

    while not rospy.is_shutdown():
        msg = CustomMsg()
        msg.header.stamp = rospy.Time.now()
        msg.robot_id = "robot_01"
        msg.speed = 1.5
        msg.angle = 0.5
        msg.battery_level = 80

        pub.publish(msg)
        rate.sleep()

if __name__ == '__main__':
    publisher()

# ROS2 Python示例
import rclpy
from rclpy.node import Node
from rclpy.executors import MultiThreadedExecutor
from rclpy.callback_groups import ReentrantCallbackGroup

class RealtimeNode(Node):
    def __init__(self):
        super().__init__('realtime_node')
        self.cb_group = ReentrantCallbackGroup()

        self.timer = self.create_timer(
            0.001,  # 1kHz
            self.timer_callback,
            callback_group=self.cb_group
        )

    def timer_callback(self):
        # 实时控制逻辑
        pass

def main():
    rclpy.init()
    node = RealtimeNode()
    executor = MultiThreadedExecutor()
    executor.add_node(node)
    executor.spin()
    rclpy.shutdown()

rospy.logdebug("调试信息")
rospy.loginfo("普通信息")
rospy.logwarn("警告信息")
rospy.logerr("错误信息")

from my_robot.msg import RobotStatus

msg = RobotStatus()
msg.robot_id = "robot_01"