春江花月夜

1
wget http://fishros.com/install -O fishros && . fishros

1
// OpenCV
2
sudo apt install python3-opencv
3
// CvBridge —— ROS 与 OpenCV 图像转换（xxx 为 Ros 的版本）
4
sudo apt install ros-xxx-cv-bridge
5
// usb 相机标准驱动
6
sudo apt install ros-xxx-usb-cam
7
ros2 run usb_cam usb_cam_node_exe

1
// pip
2
sudo apt install python3-pip
3

4
// colcon
5
sudo apt install python3-colcon-ros

1
sudo rosdepc init & rosdepc update
2
rosdepc install -i --from-path src --rosdistro xxx -y

1
ros2 pkg create --build-type ament_cmake xxx_pkg_c
2
ros2 pkg create --build-type ament_python xxx_pkg_p

1
'console_scripts': [
2
 'node_xxx       = xxx_node.node_xxx:main',
3

4
 ......
5
],

1
class HelloWorldNode(Node):
2
    def __init__(self, name):
3
        super().__init__(name)                       # ROS2 节点父类初始化
4
        while rclpy.ok():                            # ROS2 系统是否正常运行
5
            self.get_logger().info("Hello World")
6
            time.sleep(0.5)
7

8
def main(args=None):
9
    rclpy.init(args=args)                            # ROS2 Python 接口初始化
10
    node = HelloWorldNode("node_xxx")          # 创建 ROS2 节点对象并进行初始化
11
    node.destroy_node()                              # 销毁节点对象
12
    rclpy.shutdown()                                 # 关闭 ROS2 Python 接口

1
// 运行节点
2
ros2 run xxx_node node_xxx
3

4
// 查看运行节点列表
5
ros2 node list
6
// 查看节点详细信息
7
ros2 node info /node_xxx

1
class PublisherNode(Node):
2

3
    def __init__(self, name):
4
        super().__init__(name)                                    # ROS2节点父类初始化
5
        self.pub = self.create_publisher(String, "chatter", 10)   # 创建发布者对象（消息类型、话题名、队列长度）
6
        self.timer = self.create_timer(0.5, self.timer_callback)
7

8
    def timer_callback(self):
9
        msg = String()                                            # 创建一个String类型的消息对象
10
        msg.data = 'Hello World'                                  # 填充消息对象中的消息数据
11
        self.pub.publish(msg)                                     # 发布话题消息
12
        self.get_logger().info('Publishing: "%s"' % msg.data)     # 输出日志信息，提示已经完成话题发布
13

14
def main(args=None):
15
    rclpy.init(args=args)                            # ROS2 Python接口初始化
16
    node = PublisherNode("topic_helloworld_pub")     # 创建ROS2节点对象并进行初始化
17
    rclpy.spin(node)                                 # 循环等待ROS2退出
18
    node.destroy_node()                              # 销毁节点对象
19
    rclpy.shutdown()                                 # 关闭ROS2 Python接口

1
class SubscriberNode(Node):
2

3
    def __init__(self, name):
4
        super().__init__(name)                                    # ROS2节点父类初始化
5
        self.sub = self.create_subscription(\
6
            String, "chatter", self.listener_callback, 10)        # 创建订阅者对象（消息类型、话题名、订阅者回调函数、队列长度）
7

8
    def listener_callback(self, msg):                             # 创建回调函数，执行收到话题消息后对数据的处理
9
        self.get_logger().info('I heard: "%s"' % msg.data)        # 输出日志信息，提示订阅收到的话题消息
10

11
def main(args=None):
12
    rclpy.init(args=args)                            # ROS2 Python接口初始化
13
    node = SubscriberNode("topic_helloworld_sub")    # 创建ROS2节点对象并进行初始化
14
    rclpy.spin(node)                                 # 循环等待ROS2退出
15
    node.destroy_node()                              # 销毁节点对象
16
    rclpy.shutdown()                                 # 关闭ROS2 Python接口

1
class adderServer(Node):
2
    def __init__(self, name):
3
        super().__init__(name)                                                             # ROS2节点父类初始化
4
        self.srv = self.create_service(AddTwoInts, 'add_two_ints', self.adder_callback)    # 创建服务器对象（接口类型、服务名、服务器回调函数）
5

6
    def adder_callback(self, request, response):                                           # 创建回调函数，执行收到请求后对数据的处理
7
        response.sum = request.a + request.b                                               # 完成加法求和计算，将结果放到反馈的数据中
8
        self.get_logger().info('Incoming request\na: %d b: %d' % (request.a, request.b))   # 输出日志信息，提示已经完成加法求和计算
9
        return response                                                                    # 反馈应答信息
10

11
def main(args=None):
12
    rclpy.init(args=args)                            # ROS2 Python接口初始化
13
    node = adderServer("service_adder_server")       # 创建ROS2节点对象并进行初始化
14
    rclpy.spin(node)                                 # 循环等待ROS2退出
15
    node.destroy_node()                              # 销毁节点对象
16
    rclpy.shutdown()                                 # 关闭ROS2 Python接口

1
class adderClient(Node):
2
    def __init__(self, name):
3
        super().__init__(name)                                                    # ROS2节点父类初始化
4
        self.client = self.create_client(AddTwoInts, 'add_two_ints')              # 创建服务客户端对象（服务接口类型，服务名）
5
        while not self.client.wait_for_service(timeout_sec=1.0):                  # 循环等待服务器端成功启动
6
            self.get_logger().info('service not available, waiting again...')
7
        self.request = AddTwoInts.Request()                                       # 创建服务请求的数据对象
8

9
    def send_request(self):                                                       # 创建一个发送服务请求的函数
10
        self.request.a = int(sys.argv[1])
11
        self.request.b = int(sys.argv[2])
12
        self.future = self.client.call_async(self.request)                        # 异步方式发送服务请求
13

14
def main(args=None):
15
    rclpy.init(args=args)                                                         # ROS2 Python接口初始化
16
    node = adderClient("service_adder_client")                                    # 创建ROS2节点对象并进行初始化
17
    node.send_request()                                                           # 发送服务请求
18

19
    while rclpy.ok():                                                             # ROS2系统正常运行
20
        rclpy.spin_once(node)                                                     # 循环执行一次节点
21

22
        if node.future.done():                                                    # 数据是否处理完成
23
            try:
24
                response = node.future.result()                                   # 接收服务器端的反馈数据
25
            except Exception as e:
26
                node.get_logger().info(
27
                    'Service call failed %r' % (e,))
28
            else:
29
                node.get_logger().info(                                           # 将收到的反馈信息打印输出
30
                    'Result of add_two_ints: for %d + %d = %d' %
31
                    (node.request.a, node.request.b, response.sum))
32
            break
33

34
    node.destroy_node()                                                           # 销毁节点对象
35
    rclpy.shutdown()                                                              # 关闭ROS2 Python接口

1
class MoveCircleActionServer(Node):
2
    def __init__(self, name):
3
        super().__init__(name)                   # ROS2节点父类初始化
4
        self._action_server = ActionServer(      # 创建动作服务器（接口类型、动作名、回调函数）
5
            self,
6
            MoveCircle,
7
            'move_circle',
8
            self.execute_callback)
9

10
    def execute_callback(self, goal_handle):            # 执行收到动作目标之后的处理函数
11
        self.get_logger().info('Moving circle...')
12
        feedback_msg = MoveCircle.Feedback()            # 创建一个动作反馈信息的消息
13

14
        for i in range(0, 360, 30):                     # 从0到360度，执行圆周运动，并周期反馈信息
15
            feedback_msg.state = i                      # 创建反馈信息，表示当前执行到的角度
16
            self.get_logger().info('Publishing feedback: %d' % feedback_msg.state)
17
            goal_handle.publish_feedback(feedback_msg)  # 发布反馈信息
18
            time.sleep(0.5)
19

20
        goal_handle.succeed()                           # 动作执行成功
21
        result = MoveCircle.Result()                    # 创建结果消息
22
        result.finish = True
23
        return result                                   # 反馈最终动作执行的结果
24

25
def main(args=None):
26
    rclpy.init(args=args)                                  # ROS2 Python接口初始化
27
    node = MoveCircleActionServer("action_move_server")    # 创建ROS2节点对象并进行初始化
28
    rclpy.spin(node)                                       # 循环等待ROS2退出
29
    node.destroy_node()                                    # 销毁节点对象
30
    rclpy.shutdown()                                       # 关闭ROS2 Python接口

1
class MoveCircleActionClient(Node):
2
    def __init__(self, name):
3
        super().__init__(name)                   # ROS2节点父类初始化
4
        self._action_client = ActionClient(      # 创建动作客户端（接口类型、动作名）
5
            self, MoveCircle, 'move_circle')
6

7
    def send_goal(self, enable):                 # 创建一个发送动作目标的函数
8
        goal_msg = MoveCircle.Goal()             # 创建一个动作目标的消息
9
        goal_msg.enable = enable                 # 设置动作目标为使能，希望机器人开始运动
10

11
        self._action_client.wait_for_server()    # 等待动作的服务器端启动
12
        self._send_goal_future = self._action_client.send_goal_async(   # 异步方式发送动作的目标
13
            goal_msg,                                                   # 动作目标
14
            feedback_callback=self.feedback_callback)                   # 处理周期反馈消息的回调函数
15

16
        self._send_goal_future.add_done_callback(self.goal_response_callback) # 设置一个服务器收到目标之后反馈时的回调函数
17

18
    def goal_response_callback(self, future):           # 创建一个服务器收到目标之后反馈时的回调函数
19
        goal_handle = future.result()                   # 接收动作的结果
20
        if not goal_handle.accepted:                    # 如果动作被拒绝执行
21
            self.get_logger().info('Goal rejected :(')
22
            return
23

24
        self.get_logger().info('Goal accepted :)')                            # 动作被顺利执行
25

26
        self._get_result_future = goal_handle.get_result_async()              # 异步获取动作最终执行的结果反馈
27
        self._get_result_future.add_done_callback(self.get_result_callback)   # 设置一个收到最终结果的回调函数
28

29
    def get_result_callback(self, future):                                    # 创建一个收到最终结果的回调函数
30
        result = future.result().result                                       # 读取动作执行的结果
31
        self.get_logger().info('Result: {%d}' % result.finish)                # 日志输出执行结果
32

33
    def feedback_callback(self, feedback_msg):                                # 创建处理周期反馈消息的回调函数
34
        feedback = feedback_msg.feedback                                      # 读取反馈的数据
35
        self.get_logger().info('Received feedback: {%d}' % feedback.state)
36

37
def main(args=None):
38
    rclpy.init(args=args)                                  # ROS2 Python接口初始化
39
    node = MoveCircleActionClient("action_move_client")    # 创建ROS2节点对象并进行初始化
40
    node.send_goal(True)                                   # 发送动作目标
41
    rclpy.spin(node)                                       # 循环等待ROS2退出
42
    node.destroy_node()                                    # 销毁节点对象
43
    rclpy.shutdown()                                       # 关闭ROS2 Python接口

1
# 通信数据
2
int32 x
3
int32 y

1
# 请求数据
2
int64 x
3
int64 y
4
---
5
# 应答数据
6
int64 s

1
# 目标
2
bool enable
3
---
4
# 结果
5
bool finish
6
---
7
# 反馈
8
int32 state